MQ 52392

A New Digital Differential Analyzer Approach for
Inert ial Technologies

by
Nelson L. P. M. Monnerat
-4 thesis submitted to
the Faculty of Graduate Studies and Research
in partial fulfillment of
the requirements for the degree of
hiaster of Engineering
Ottawa-Carleton Institute for
Mechmical and Aeros~ace Ennineerine
Department of
Mechanicd and Aerospace Engineering
Carleton University
Ottawa, Ontario
May, 2000
@Copyright
2000, Nelson bfonnerat
"1
uisitions and Acquisitions et
Bi iographic Services setwices bibliognphiques
The author has granted a non- L'auteur a accord une licence non
exclusive Licence aiiowing the exclusive pennettant la
National Library of Canada to Bibliothque nationale du Canada de
reproduce, loan, distribute or seii reproduire, prter, distribuer ou
copies of this thesis in microform, vendre des copies de cette thse sous
paper or electronic formats. la forme de microfiche/film, de
reproduction sur papier ou sur format
lectronique.
The author retains ownership of the L'auteur conserve la proprit du
copyright in Uiis thesis. Neither the droit d'auteur qui protge cette thse.
thesis nor substantial extracts fiom it Ni la thse ni des extraits substantiels
may be printed or othe~vike de celle-ci ne doivent tre imprims
reproduced without the author's ou autrement reproduits sans son
permission. autorisation.
Abstract
This thesis presents a new approach to digitizing and implementing the navigation
algorithm for the Heading and Reference Unit (HRU) under development at the Defense
Research Establishment Ottawa (DREO), based on the Digital Differential Analyzea
(DDA) .
An inertial strapdown HRC' usually needs a dynamic range of 0.01 deg/h to 400
deg/h for most appiications, which corresponds to a voltage range of 1pV to 10V. For
this dynamic range a 20 bit resolution with an output rate of 1Hz up to 200Hz is needed
[Fejes 941. The DDA technology is an approach to solve this requirement.
The DDA is the digital approach for the analog differential analyzer. Its basic
element is the integrator, which is similar to the analog integrator, but uses counters and
adders to calculate the integral of a differential equation or set of differential equations.
The use of the DD.4, when implemented in hardware, allows for the implementation
of the navigation algorithm to be performed in parallel .
In this thesis the self-alignment and navigation algorithm proposed by Caroie
Bolduc in "Self'rllignment and Navigation Algorit hms for DREO Navigation Labo-
ratory Heading Reference Unit" are implemented using the DDA approach.
Acknowledgement s
My deepest thanks are extended to my supervisor Dr. Douglas A. Staley for his invalu-
able advice and supervision through the course of this research. and to my cesupervisor
Prof. John Sinkiewicz.
1 would also like to express rny thanks to Michael Vinnins, Derek Hartl and Lloyd
Gallop for their assistance and advice and for making this project possible.
The author also wishes to thank the Mechanical and Aerospace Engineering office
st af f and the Writing Tutorial Service for their assistance.
Finally? 1 would like to express my deepest appreciation to my wife and sons, this
work would not have been compieted without their endless encouragement and patience.
Thank You Lord for giving me the strength and guidance.
1 dedicate this work to my wife and sons.
Contents
Abstract
Acknowledgments
List of Figures
List of Tables
List of Abbreviations
List of Symbols
i
ii
vi
viii
X
xii
1 Introduction 1
. . . . . . . . . . . . . . . . . . . . . . . . . 1.1 Motivation and Objective 2
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.2 Thesis Outline. 3
2 HRU and The Reference Rames 6
. . . . . . . . . . . . . . . . . . . . . . . . . . 2.1 Description of the HRU 6
2.2 Reference Coordinate Frames . . . . . . . . . . . . . . . . . . . . . . . 7
3 Digital DifFerential Analyzers (DDA) 11
iii
CONTENTS
3.1 Digital DifFerential hal yzer Principles . . . . . . . . . . . . . . . . . . 12
3.2 Incrementai Computation . . . . . . . . . . . . . . . . . . . . . . . . . 13
3.3 TheIntegrator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
3.4 Programming the DDA . . . . . . . . . . . . . . . . . . . . . . . . . . 18
3.1.1 The Integrator . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
3.4.2 Flow Diagram Example . . . . . . . . . . . . . . . . . . . . . . 19
3.1.3 Scaling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
3.5 Basic htegrators . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
3.6 Error.4nalysis. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
3.6.1 Basic Concepts . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
3.6.2 Errors in the DD.4 . . . . . . . . . . . . . . . . . . . . . . . . . 30
. . . . . . . . . . . . . . . . . . . 3.7 Sample Data Theory Applied to DDA 33
3.7.1 Definitions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
3.7.2 2-Transform . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35
3.7.3 The Integrator . . . . . . . . . . . . . . . . . . . . . . . . . . . 36
4 HRU Algorithms lmplemented in DDA Form 38
. . . . . . . . . . . . . . . . . . . . . . . . . . 4.1 HRC .4l ignment Scheme 39
4.2 Compensation algonthms . . . . . . . . . . . . . . . . . . . . . . . . . 42
. . . . . . . . . . . . . . . . . . . . . 1.2.1 Acceleration compensator 44
. . . . . . . . . . . . . . . . . . 4.2.2 Angular Velocity Compensator 47
. . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.3 Xatigation Algorithm 54
. . . . . . . . . . . . . . . . . . . . . . . 4.3.1 Damped Spatial Rates 54
. . . . . . . . . . . . . . . . . . 4.3.2 The Direction Cosine Matrix Q 59
4.3.3 The Direction Cosine MatrLx D . . . . . . . . . . . . . . . . . . 62
CONTENTS
4.3.4 True .b gular Velocity . . . . . . . . . . . . . . . . . . . . . . . 67
4.3.5 True -4cceleration . . . . . . . . . . . . . . . . . . . . . . . . . . 70
4.3.6 Quatemions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72
-c
. . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.3.7 Tme Velocity ra
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.4 Orthogonalization 79
4.4.1 Quaternions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 80
4.4.2 Direction Cosine Matrices . . . . . . . . . . . . . . . . . . . . . 82
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.5 Constraints 85
5 Software Implementation 94
5.1 Software Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . 96
5.2 Configuration Files . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 98
6 Results 99
6.1 Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 100
6.1.1 Ideal Sample Data . . . . . . . . . . . . . . . . . . . . . . . . . 101
6.1.2 Static Vehicle Sample Data . . . . . . . . . . . . . . . . . . . . 102
7 Conclusion 107
7.1 Future Research . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 108
A Analog to Digital Converters 113
B Description of the Simulation Software Files 129
C Configuration Files
List of Figures
1.1 Inert i d navigation syst em (INS) diagram . . . . . . . . . . . . . . . . . 2
2.1 Reference coordinate frames . . . . . . . . . . . . . . . . . . . . . . . . 8
3.1 Digital Integrator with respect to time . . . . . . . . . . . . . . . . . . 16
3.2 Digital integator with incremental output . . . . . . . . . . . . . . . . 17
3.3 The integrator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
3.4 Second-order differential equation . . . . . . . . . . . . . . . . . . . . . 20
3.5 DDA multiplier . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
3.6 DDA square implementation . . . . . . . . . . . . . . . . . . . . . . . 26
4.1 Damped Platform Alignment Mode1 for the North-&sis .Accderometer . 41
4.2 DDA implementation of the acceIeration compensator . . . . . . . . . . 45
4.3 DDA Implementation of the angular velocity compensator . . . . . . . 50
4.1 DDA implementation of damped angular rates . . . . . . . . . . . . . . 56
4.5 DDA implementation of the DCM Q terms . . . . . . . . . . . . . . . 61
1.6 DDA implementation of the DCM D . . . . . . . . . . . . . . . . . . . 65
1.7 DDA implementation of the true angular velocity . . . . . . . . . . . . 68
4.8 DDA implernentation of the tnie acceleration . . . . . . . . . . . . . . 71
LIST OF FIGURES
4.9 Simple DDA integrators . . . . . . . . . . . . . . . . . . . . . . . . . . 73
4.10 DDA implementation of the quaternions . . . . . . . . . . . . . . . . . 74
4.11 DDA Implementation of the tme velocity . . . . . . . . . . . . . . . . 76
4.12 DDA implementation of quaternion's orthogonalization . . . . . . . . . 81
4.13 DDA Implementation of the error term of the orthogonalization of the
DCM elements . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 84
6.1 Results of the first simulation . . . . . . . . . . . . . . . . . . . . . . . 104
6.2 Results of the simulation with HRU's sample file . . . . . . . . . . . . . 106
.2.1 Charge reset VJF converter [Hoeshele 941 . . . . . . . . . . . . . . . . . 123
A.2 Charge balancing V! F convener [Hoeshele 941 . . . . . . . . . . . . . . . 125
List of Tables
3.1 Xumerical example of digital integation . . . . . . . . . . . . . . . . .
3.2 Scaling table for the second-order difl'erential equation . . . . . . . . .
3.3 Common Z.Transforms . . . . . . . . . . . . . . . . . . . . . . . . . . .
3.1 2-Transform Properties . . . . . . . . . . . . . . . . . . . . . . . . . .
1.1 Acceleration compensator integrator scale factors . . . . . . . . . . . . .
. . . . . . . . . . . . . . 1.2 Parametea for the angular velocity integrators
. . . . . . . . . . 4.3 .4nguiar velocity compensator integrator scale factors
4.4 Damped spatial rates integrators' parameters . . . . . . . . . . . . . .
4.5 Integrator scale factors referent to the Q matrix diagonal elements . .
4.6 Integator scale factors referent to the Q matrix non-diagonal elements
4.7 Integrator scale factoa of the DDA implementation of the DCM D . .
4.8 Integrator sal e factors of the DD.4 implementation of the true angular
vebcity . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . c c .
4.9 Integrator scale factors of the DD.4 implementation of the true acceler-
ation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
4.10 Integrator scale factors of the DD.4 implementation of the quaternions
4.11 DDA integrator scaie factors of the true velocity . . . . . . . . . . . .
- LIST OF TABLES
4.12 DD.4 integrator scale factors of the orthogonalization of the quaternions 81
4.13 DDA integator scale factors of the orthogonalization of the DCM terms 85
4.14 Table of algorithm constraints . . . . . . . . . . . . . . . . . . . . . . . 86
1.15 Relations between the size of the registers and the supremum of registers 92
6.1 Configuration file hru. cfg . . . . . . . . . . . . . . . . . . . . . . . . . 103
List of Abbreviations
BLT
CEP
D/A
DCM
DDA
DREO
EPLD
FPGA
GPS
HRL
13s
.halog to Digital Converter
American Xational Standards Institute
Extention of the Tk toolkit
Circula Error Probable
Canadian Strapdown Gyroscope
Digital to Analog Converter
Direction Cosine Matrix
Digital Differential.4nalyzer
Defense Research Establishment Ottawa
Erasable Programmable Logic Device
Field-Programmable Gate . k a y
Greenwich Mean Time
A recursive acronym for "GNG's Not Un*'
Global Positioning System
Graphical User Interface
Heading Reference Unit
Inertial Navigatian System
Inertial Se mr Assembly
LIST OF ABBREVIATIONS
LPF
MSB
PCM
PMW
R,VS
RTD
ROC
SBC
S/H
SPU
SQNR
TIIJORC'
Low Pass Filter
Most Significant Bit
Pulse Code Modulator
Python Megawidgets
Root Mean Square
Resistance Thermal Device
Region of Convergence
Single Board Computer
Sample and Hold circuit
Signal Processing Unit
Signai-to-Quantization Noise Ratio
The lnenial Navigation Organized Research Unit
List of Symbols
amax
air
Supremum of compensated acceleration
Raw data h m the accelerometers for the '5'' axis
Supremum value of a,
Compensated acceleration for the 'Y axis
Acceleration dong the gyroscope axis 'Li"
-4ccelerometer misalignment angles, the first su bscript refers to the body
axis and the second refers to the system axis
Xccelerometer bias drift for the '5" axis
True acceleration in computational frame of reference
Accelerometers scale factor, where i = { x, y, z)
DCM that transforms from systern frame to computational frame as func-
tion of Eler angles
DCM that transforrns from earth-centered earth-hed frame to computa-
tionai frame
Quantization error
Local Geographical Frame (East, Yonh, Lp)
Gravity acceleration, 9.806m/s2
Geograp hic Align Matrix
xii
LIST OF SYMBOLS
Gyroscope misalignment angles, the k t subscript refers to the body axis
and the second refea to the system axis
Gyroscope bias drift for the 'Y' axis
Gyroscope scale factors, where i = {x, y, z)
Integral register
is the input of the Y register
Terms defined in section 4.2.2 for the angular velocity compensation
Acceleration compensator integrator scale factor
True acceleration integrator scale factor
DCM D terms integrator scale factor
Quaternions integrator scale factor
DCM Q tenns integator scaie factor
True velocity integrator scde factor
True angular velocity integrator scale factor
Damped angular rate integrator scaie factor
Angular velocity compensator integator scale factor
Number of quantization levels
Gyroscope mass unbalance drift term for the 'Y aaxis
Supremum of true acceleration
Supremum of DCM D terms
Supremum of quaternions
Supremum of DCM Q
Supremum of true velocity
Supremum of true angular
LIST OF SYMBOLS
Supremum of damped angular rate
Supremum of the integrand variable y
Supremum of the AK terms
Supremum of the K, te=
Supremum of the GK terms
Number of bits in the integrator registers
Integrator register capacity
Acceleration compensator integrator register capacity
True acceleration integrator register capacity
DCM D terms integrator register capacity
Quaternions integrator register capacity
DCM Q terms integrator register capacity
True velocity integrator register capacity
True angular velocity integrator register capacity
Damped angular rate integrator register capacity
Angular velocity compensator integrator register capacity
is the output of the integral
Mean square error power
Quaternions
DCM tthat transforms bom system frame to computational frame as func-
tion of quaternions
Gyroscope quadrature drift t e m for the 'Si" axis
RadUr of the DDA operator
Integral Register
LIST OF SYMBOLS
& Earth's radius
rn the round-off
Rs A/D range
G
Tme velocity in the computationai frame of reference
VR Reference voltage
XU Analog signal
Xq Quantized value of x
x Analog variable of interest
Xi, Yi, Zi
Earth-centered inertial frame
Xe, k;, 2, Earth-centered earth-fixed frame
, Y , Z Local-level wander-azimuth frame
X,Y,Z Integrators variables
Y', R',
Conesponding integrator's registea updated
Q! Wander angle
6x Increment
A Quantizat ion level
CD& Latitude
X Longitude
Ct c
True angular velocity in the computational fraxne of reference
P
Damped rate in the computational frame of reference
fi .hgular rate of the cornputational frame of reference with respect to the
eart h-centered eart h-ked frame of reference
OZ
Standard deviation of the signal x
T Damping parameter
LIST OF SYMBOLS
C
1 Q
Interval where x, is between quantization levels
w Compensated angular velocity
~ m a
Supremum of compensated angular velocity
UT Raw data fiom the gyroscopes
ni, Earth anguiar ratio in the earth-centered inertid frarne of reference
0, Earth angular ratio in the computational frsme of reference
Chapter 1
Introduction
Inertial navigation and guidance systems are applied to determine the physicd position
and velocity of airerafts and ships relative to a reference fiame, such as their geograph-
ical position. They are also used to guide the vehicle frorn one point to another. The
advantage of the inertial navigation over radar, sonar, or GPS (Global Positioning Sys-
tem) is that it is completely independent of extemal information or influences such as
weat her, magnetic disturbances, electronic jamming and distortion.
Modern inertial navigation and guidance systems consist of an inertial sensor as-
sembly, a signal processing unit and navigation algorithms. A typical inertial sensor
assembly consists of three linear accelerometers and their associated electronics, and ei-
ther three single-degree-of-freedom gyroscopes, or two 2-degree~~freedom gyroscopes.
An inertial navigation systern is calleci gimbaled when it uses a stable platform with
three or more gimbals, which is kept either nonrotating with respect to an inertial
frame, or establishes a reference hune which precesses from the inertial frame at a
hown rate [Siouris 93).
CHAPTER 1. INTRODUCTION 2
A strapdown inertial navigation system dinen from a gimbaled .stem in that the
inertial sensors are mounted directly ont0 the vehicle frame instead of on a stable
platform. The navigation cornputer keeps track of the vehicle's attitude, with respect
to some preselected reference frame, based on information from the inertial sensoa.
The gyroscopes' output is used to either drive the gimbal torque motors in such
a way as to maintain the platform in alignment with the inertial coordinate rame
in a gimbaled system, or to keep the computational frame oriented in a strapdown
system. The output of the accelerometers is transformed to the computational frame.
Perfonning computations through the navigation algorithm generates the navigation
information .
1.1 Motivation and Objective
In a very simple schematic approach, an inertial navigation system can be represented
by the diagram in figure 1.1.
Figure 1.1: Simpie schematic diagram of an inertial navigation synem (IXS).
C
Gyroscopes and
~cceler~metm
The interface to the sensors is digital, so the information can then be directly applied
by other systems that are intepated to the vehicle.
An inertial navigation system, such as the strapdown Heading Reference Unit (HRC),
0
A
RebaJan
I
Digitisers
4 L
D
-
>
Navigation
~~goriutm
LOOP
usually needs a dynamic range of 0.01 degjh to 400 deg/h for most applications, which
corresponds to a voltage range of 1pV to 10V. For this dynamic range a 20 bit resolution
with an output rate of 1Hz up to 200Hz is needed [Fejes 941.
Different techniques have been applied to digitize the sensor signals. Reference
[Fejes 941 presents an approach for digitizing the signal from the HRC' from the Defense
Research Establishment Ottawa (DREO) using Delta-Sigma analog-to-digital convert-
ers (A/D) with resolution of 21 bits.
The Ai D presented in [Fejes 941 was used in reference [Bolduc 951, where the navi-
gation algorithm for the Heading Reference Unit (HRU) from DREO is presented.
As presented in [Hartal 971 and [Hartl-1 971, the stability of the least significant bits
caused the reduction of the resolution of the A/ D to 17 bits, and motivated the design
of new .4/D boards and configurations to be implemented in the HRC using new A/'D
converters.
The objective of this thesis is to improve the precision and the fiequency response
of the HR' system using the Digital Differential Analyzers (DDA).
The algorithm development and testing has been done by Carole Bolduc and is
very well presented in her work [Bolduc 951. In this work. her results will be used as a
reference for cornparison, since the HRC hardware irnplementation does not allow the
DDA implementation and testing, which would require frequencies around 500 kHz.
1.2 Thesis Outline
This work has been organized as foilows:
Chapter 1 Introduction, where a very brief overview of this work is presented.
Chapter 2
Chapter 3
Chapter 4
Chapter 5
Chapter 6
Chapter 7
The frames of reference used by the navigation algorithm are presented.
Also the HRU system is briefly described.
In this chapter the DD-4 is presented, its basic concepts and principles, and
how they interact with each other. Some basic building blocks that are used
to implement the navigation algorithm are also presented.
In this chapter the navigation algorithm developed by Carole Bolduc is
implemented in a DDA form. Each section corresponds to a part of the al-
gorithm, beginning with a description of the algorithm and then their DDA
implementation. The final section of this chapter presents the relations of
each part of the algorithm and a more general description of the parameters
is deduced. These relations are implemented in the simulation software as
constraints or to find the parameter values.
An explanation of how the simulation software has been built is presented
in this chapter, with a description of each file and classes that implement
the algorithm components. Since the software has around 10,000 lines, the
source is not part of this work, but only the description of the classes'
implement ations.
The results achieved h m the same data files used by Carole Bolduc in her
work are presented in this chapter. This has been done in order to allow a
cornparison between these two works, since the DD.4 was not implemented
in hardware.
This is the fuial chapter where the conclusions are ~resented with oro~osals
CHUTER 1. INTRODUCTION
for future work.
Chapter 2
Description of The HRU and The
Reference Frames of Coordinates
In this Chapter the description of the Heading Reference Unit (HRC) from DREO will
be presented. The parameters used in this work are from this system. The description
that follows is based on the reports in references [Hartd 971 and [Hanl-l 971.
-41~0 a description of the reference frames of coordinates used through out this work
will be presented.
2.1 Description of the HRU
The HRU is designed to be a stand-alone strapdown attitude and heading reference
unit with full navigation capabilities. The intended accuracy of the system is to be of
0.1 degree RMS in roll, pitch and yaw, and Iess than one nautical mile pet hour Circular
Error Probable (CEP) in position [Hartl-1971.
CK4PTER 2. HRU AND THE REFERENCE FRAMES 7
CEP is a concept derived fkom the accuracy of the delivery of a weapon such as
bombs or missiles. It is defined as the radius of a circle centered at the mean point
of impact that encloses 50% of the probability of a hit. Since CEP has the error
distribution information in its formulation, it can be stated that the smaller the CEP,
the smaller the errors? and the more accurate the system. More detailed information
about the CEP and its applications can be found in reference [Siouris 931.
The HRU is comprised of two separate hardware assernblies, the Inertial Sensor
Assembly (ISA) and the Signal Processing Unit (SPU). The IS.4 is a six inch cube
in which three Q-Flex QA-700 penddous accelerometea and two CSG-2 dynamically
tuned two-degree-o'freedom gyroscopes are mounted. The block is thermally controlled
and monitored using cartridge heaters and resistance thermal detectors (RTD) mounted
on the block. The 1s-4 is connected to the SPU via shielded cables.
The SPC is a standard rack-mountable 19 inch cardcage which houses dl the signal
processing, supply and control electronics. Signals from the seoson are igitized in the
SPC. The 486 single board cornputer (SBC) reads data over the backplane bus.
2.2 Reference Coordinate Frames
There are sever al dinerent re ference coordinate rames available to the inert ial naviga-
tion system designer for use in navigation computation of position, velocity and attitude.
The reference frarnes presented in this section are the ones used in Carole Bolduc's work
[Bolduc 951 which follows the same approach as [Britting 7 11 and [Siouris 931.
Geographic Rame (n) the geographic f r ame is a local navigational frame (also cdled
navigation frBme) that has its origin at the HRU location and ts axes can be
CHAPTER 2. HRU AND THE REFERENCE FRAMES
Figure 2.1: Reference coordinate kmes used through out this work.
Xi, Yi, Zi
earth-centered inertial fiame
Xe, Y,, 2, earth-centered earth-6xed frame
E, N, U local geographic frame
&, Y,, 2, local-level wander-azimuth frarne
CHAPTER 2. HRU AND THE REFERENCE FR4MES 9
defined either as North, East and Down or Eastl North and Up directions. The
Up (U) and Dom (D) axes are defined as normals to the reference eliipsoid. The
North axis (N) is in the direction of the projection of earth's inertial anguiar
velocity vector into the local horizontal plane. The East direction completes the
right-handed orthogonal set. This is a local-level frame with its x and y axes in a
plane tangent to the reference ellipsoid. The convention adopted in this work is
East=X, North=Y and p=Z.
Body R a m e (b) the body m e (also called system frame) consists of the roll, pitch
and yaw axes of the vehicle with its ongin at the center of mass of the vehicle.
In general, the vehicle's center of mass does not coincide with the location of the
Attitude and Heaing Reference ystem. It is a convenient coordinate system
for developing the equations of motion of a vehicle, since the vehicle's motion
equations are nomally written in this frame of reference. The relations between
the sensors, gyroscopes and accelerometers, frames of reference and the body
h e must be determined for each specific application.
Local-Level Wander- Azimuth Rame ( c) the local - level wander -ozimuth fmme is
also referred to as the computational or navigational frame. Its origin coincides
with the geographic frame. The horizontal axes lie in a plane tangent to the
local vertical. Eulerian rotations of longitude, geodetic latitude and wander angle
are used to relate the locai-level wander-azimuth frame to the earth-centered
earth-fixed frame. The latitude ( #i ) rotation is defined to be positive in the
northem hemisphere, the wander angle (a) is defined positive West of true north
and measured in the geodetic horizontal plane, and the longitude (A) is defined
CHAPTER 2. HRU AND THE REFERENCE FFL4MES 10
positive east of the Greenwich meridian and is rneasured in the equatorid plane.
When cr the wander angle is equal to zero the local-level wander azimuth frame
is coincident with the geographic frame.
Eart h-Centered Eart h-Fixed Rame ( e) the earth-centered earth-jked fmme has
its ongin at the earth's center of mas . This frame rotates with the earth. The
Y, axis is directed North dong the eaxth's spin ais, while the Xe and 2, axi s
are in the equatorial piane with X directed through the Greenwich Meridian (O0
latitude, O0 longitude), and the Y axis is directed 90 east longitude.
Earth-Centered Inertial Rame (i) the earth-centered inertial frame has its origin
at the eanh's center of mass and is non-rotating relative to the inertial space (Le.
the fked st an) . However, this frame accelerates with respect to inertial space
since it moves with the earth. It coincides with the earth-centered earth-fixed
frame once every 24 hours. The Xi and Zr axes lie in the earth's equatond plane,
the Yi axi is aligned with the earth's spin axis.
Chapter 3
Digital Differential Analyzers (DDA)
The Digital DifFerential Analyzer (DDA) was applied in the 1960's for reai time op-
eration, combining the speed of analog machines with the accuracy and flexibility of
digital ones. It has also been applied as a solution for navigation and attitude control
in some Inenial Navigation Spstems. However, large increases in the computing speed
and the considerable reduction in size of microprocessors led to the relegation of the
DDA to historical interest only.
In order to understand the DDA, some concepts related to analogue computea need
t o be addressed. Generally, analogue cornputers fall into two categories, differential
analyzers and simulators. The differential analyzer solves a panicular equation or set of
equations within a range of coefficients. The simulator, on the other hand, is a detailed
working mode1 of a physical system, which enables its behavior to be examined under
a wide range of conditions.
The DDA is the digital approach for the analog dinerential anaiyzer. Its basic
element is the integrator, which is similar to the analogue integrator, but uses counters
CHAPTER 3. DIGITAL DIFFERENTIAL ANALYZERS (DDA)
and adders to calculate the integral, as will be shown in section 3.1.
?iowadays, one can find onlp a few DDA applications where incremental integration
with high accuracy and real time response is needed, such as numerical control of
machine solutions for interpolators [Chai 941, and inverse solutions for robot kinematics
using incremental integration through a DDA [Park 941.
3.1 Digit al Differential Analyzer Principles
-1 DDA consists of a set of integrators mechanized by a digital process. The DDA
integrator is an incremental device with its principle of operation based on the hypoth-
esis that phpi cd -stems change only by a small amount in a small interval of time
[Sizer 681.
Following the approach in [Sizer 681, to present the basic principle of the DDA, let
the increment bX alweys be small, then
will be a close approximation to
so the integal can be approximated as a successive sum of smail increments.
In order to allow the DDA to be connected to other integrators, it is arranged so
that the output A 2 of any integrator may be used as the incremental input, either as
AX or 4 YI of any other. With the adder approximation of an integral as shown above,
the resuit of the addition will be
where n is the number of bits in the Y register, and r is the radix of the numb ers on
which the DD.4 operates, usually T = 2. In section 3.3 it will be described how it is
implemented in such a way that the output of the integrator is in increment form using
correct scaling.
3.2 Incremental Computation
An important property of physical systems is that they behave in a continuous manner,
which means that values change a smail amount in a small intervai of tirne. So if the
state of the system is known in one instant of time, it is only necessary to calculate a
small change, which occua during an iteration period, to obtain the state at the next
instant. The time fiequency in which it is calculated shodd be high enough to sati*
the Xyquist condition avoiding alias.
With the approach usuaily used for digital cornputers, a succession of discrete values
is calculated in each iteration. To illustrate the principle of incremental computation,
an exampie from [Sizer 681 will be shown.
Suppose that the value of the product of the variables X and Y is known at a given
instant of time
To calculate the value after one iteration, where X' = X + bX and Y' = Y + 6Y,
the new product d u e will be
Assuming that the values of the increments are sufficiently small that the second order
t em can be neglected, the product change can be approxirnated to
and this is the increment that should be calculated.
At bt sight it seems that, instead of one multiplication, now there are two multi-
plications and one addition. However, if by suitable scaling and sufficiently shortening
the iteration period, each of the variables never changes by more t han one bit, or in
1 1
other words, 6X and bY are limited to values - O and --
2" '
. where n defines the size
2" '
of the digital word. Then the multiplication is reduced to merely adding or subtracting
X and Y at a suitable level of significance.
CHAPTER 3. DIGITAL DIFFERENTIAL ANALYZERS (DD.4)
3.3 The Integrator
One of the fundamental premises for the DD.4 integrator is that the increment 6X
should always be s md, in order for the expression Y.6X to be a good approximation
of S Y.dX .
To show the principle of the DDA integrator, let bX be constant and the srnailest
representable digit, that is let bX be one bit. Dunng one iteration period K6X is the
area of the corresponding rectangle, and because 6X is one, the area is equal to Y.
With these premises, an integrator as presented in figure 3.1 can be built. It consists
of two digital registers, called Y and I. The Y register holds the current value of the
integrand and the I register holds the current value of the integral. During each iteration
two operations are made:
a The value of the integrand Y is updated to Y' by adding the increment of Y
The current value of Ya.6X is added to I to f om the current value of the integral
I'
However we have assumed that bX is equal to 1, in the more general case, using a
correct scale' , bX can assume the values (-1, O, + 1). The rules for the integration
process wiI then become:
-
'Sec section 3.4.3 for more details.
CHAPTER 3. DIGIT-4.L DIFFERENTIAL ANUYZERS (DDA)
w
Y
A
6X
add Y i f 6X 1
subtractYif&X=-1
do nothing if oX = O
1
.
Figure 3.1: Diagram of the digital integrator with respect u, t he.
Subtract Y if 6X is -1:
Do nothing if 6X is O.
Table 3.1: Numencd example of how a digital integrator register R is implemented in order to
generate outputs in a incremental form.
In order to allow the integrator to be interconnecteci to others integrators, the
integral should be modified to a incremental form, such as its inputs, instead of a whole
number f om in the register I. To show how it is done [Sizer 681 presents an example
through the table 3.1
6V
Figure 3.2: Digitai integrator with incrementai output generated as an overflow of the Uiregrd
register R.
-
Y
6X
addY #SIC-1
1 subtract Y if 5X = -1
do nothing if SX II O
It could be noted that, while the integrand Y consists only of a single integer, the
R
integrai builds up to an integer with double the size of Y. Thus the part of I of interest
Integral
is the most significant half, and this represents the value of the integral to some scale.
overflow Bw L
So the digital integrator register I should be divided into two haives as shown in figure
3.2.
The final form of the integrator is a device consisting of two equal length registers? Y
and R. The d u e of Y is kept according to Y' = Y + W at each iteration. The current
value of Y is multiplied by the increment bX, which can take the values (-1, 0, 1)?
and is then added to the register R. This causes the register R to overflow periodically.
The overflow represents the increments of the integral as shown by the equation
CHAPTER 3. DIGITAL DIFFERENTIAL AXALYZERS (DDA) 18
Now the interconnection of integrators is made possible and it could be used to solve
problems in the same way as analogue cornputers.
There are two main ways in which DDA may be organized. There is the type in
which each integrator is a self-contained unit and the integration process takes place
simultaneously in al1 integrators. This type of DDA is known as 'simultaneous'.
In the other type of DDA organization the Y and R values are kept in memory
registers and read out in turn to undergo the integration process in a common arithmetic
unit. This is known as the sequential type of DDA.
In [Sizer 681 there are some examples and a more detailed explanation of these
implementations.
3.4 Programming the DDA
3.4.1 The Integrator
The principles of the integrator have been described in section 3.1. It will nocv be
treated from the point of view of programming, consisting of flow charting and scaling.
The notation that will be used has been proposed in [Sizer 681 as shown in fig-
ure 3.3 (a). The inputs are one 4 X point, dx, and a 4Y point capable of accepting
dyl, dg2, dg3, increments. There is a 42 output providing several equivalent out-
puts dr. Although severd d t outputs and dy inputs are shown, usually in 0ow c h ms
it will appear as in figure 3.3 (b) with only one output and only one input. Yote that
the small circle means that the output is changed from +dz to -dr.
Each integrator is defined by a unique number, j, in the diagram.
a) General representation b) Integrator with sign change
Figure 3.3: The integrator representation.
3.4.2 Flow Diagram Example
-4 problem is presented for solution using DDA in the form of either one differential
equation or, possibly, several simultaneous differential equations.
The input to the register Y of an integrator represents the derivacive of the quantity
in that register. For example, consider the secondsrder differential equation
dy
If the solution is required for y and -, they must each be held as the Y register
dx
of an integrator. The input to these two integrators are the respective derivatives of y
dy
and -. Rewriting the original equation it will be
dx
multiplying the equation by dx gives
d (2) = -ddy - Bydz
which is the equation linking dy and d (2).
Based on equation 3.9 the fiow diagram in figure 3.4 can be built.
Figure 3.4: DDA mode1 of a second-order differential equation.
3.4.3 Scaling
Integrator Scale Factors
In order to use the variables x? y and z in a DD.4 integrator. each one should be
represented by X, Y and 2, where each increment corresponds to one bit. The following
relations should also be true:
in such a way that Kz K, and Kz represent the change in x, y and z produced by one
increment . K, , Ky and K, are known as the scafe constants or integmtor scale factors.
Suppose an integator with variables dz and dy for AX and AY, respectively, and
CHAPTER 3. DIGITAL DIFFERENTIAL kNfiYZERS (DD.4)
let dz be such that
so y is obtained by integrating dy into the register Y.
If the capacity of the integrator is X, the basic integrator equation will be
taking the scale factors K, , K, and Kz the equation (3.11) will be
sing equation 3.12 with 3.13, the relation between the scaling constants can be
found as
Now let M be the supremum2 of y, and let y,,, be the maximum value the problem
variable y can have, then
ZLet G be the set of possible values of the variable y, a number 1Mis called the IoJt upper bound
(Lu-b) or the supremum (sup) of G if y M for al p E G [Friedman 711.
fiom equation 3.14
Scaling Table
When the configuration is simple, like an integrator loops back AZ as AY, the solu-
tion to equation 3.14 will give the scale constant values. Usuaily there is insuflicient
information to solve equation 3.14 alone and equations 3.15 and 3.16 should be used to
solve for each integrator. In order to solve these equations reference [Sizer 681 proposes
a convenient way of constructing a table in conjunction with the flow diagam. This
table is called the int egrator scaling table.
To construct the integrator scaling table. the integrators are nurnbered, and these
numbers are transfered to the table, with a line corresponding to each integrator. The
table should have the following columns:
a The integrator numbers (j);
a The quantity (in problem variables) held in the register Y;
a The number of the integrator ( 2 ) whose output is the source of ( j ) ' s hX input:
or AT if the hX source is machine t h e , or the quantity in problem variables;
a The numbers of the integrators whose outputs are the sources of 4Y inputs to
(j), or the quantity in problem variables;
a The maximum value that y can have in problem variable form y, ;
a The maximum value calculated for y in machine variable form M;
CHAPTER 3. DIGITAL DIFFERENTM iW4LYZERS (DD-4)
The scale constant on the AX input Kz;
0 The scale constant on the A Z input Kz;
a The scale constant on the AY input Ky;
a The register length of the integator N.
When al1 the columns have been filled the scaling is complete. It may be necessary
to insert arbitrary values of Kzt Kv, or Kz if there is insufficient knowledge about the
problem, and from there work out N and iM for each integrator.
Using the second-order differential equation represented by the flow diagram in
figure 3.4, assuming that A = 0.271 and B = 12.& O < z < 8 and y is represented to
one part in 1,000. the corresponding scaling table is presented in table 3.2.
1 Integrator number / Y register 1 A X 4Y y , / iM Kz 1 K, K, N
1 !Y F AT 3? 4 - 22 2-12 / 9-10 - 28 210
4 12.3 (B) 1 2 - 12.3 2* 2-l2 1 24 24-n 1 2"
Table 3.2: Scaiing table for the second-order differential equation modeled in figure 3.1.
To fil1 the table 3.2:
1. Fill the left column with integrator numbers in order.
2. Fil1 the next column, 'Y register", with the respective contents descnbed in prob-
lem variables.
3. Fill the columns "4X" and "LW" refemng to the flow diagram.
4. The y, may be inserted if known.
CHAPTER 3. DIGITAL DIFFERENTIAL AN.4LYZERS (DDA) 24
3. The N value may be inserted, for our example of second-order differential equation
with one part in 1,000 is chosen N = 1024 = 21. The other values should be the
maximum capacity of the register, in the example 2".
6. Consider each integrator applying the equation 3.14, in the example:
7. Consider each integrator applying the equation 3.16, in the example:
8. Look from the flow diagram the relations between the integraton. In figure 3.4
the output of (1) is the 4Y input of (2) and the AX input of (3), which means
that:
CHAPTER 3. DIGITAL DIFFERENTIAL ANALYZERS (DDA) 25
From these relations and the equations 3.14 and 3.16, the scale constants are found and
the corresponding columns filled.
In the following integrator scaling tables of the navigation algorithm implementa-
tion, for the sake of simplicity, the y, column will be suppressed.
Refermce [Sizer 681 shows simple sub-programs for ezponential loop, sine-cosine
loop, arcsane and arccosine, product, square, square-root, quotient and adders. . Go,
other forms of implementing setvo elements, integators as decision units and as store
elements are presented.
The following section will present simple integrator implementations that wi l l be
used through out this work.
3.5 Basic Integrators
This section presents basic integrators that are used as building blocks for the imple-
mentation of the navigation algorithm in chapter 4.
Multiplier
Let v = g.h so the differential of v is bv = 6g.h + g.6h. In a DDA form this can be
implemented as in figure 3.5.
Figure 3.3: This is the DDA implementation of the merenrial of the multiplication of two funcrion.
CHAPTER 3. DIGITAL DIFFERENTL4L ANALYZERS (DDA)
Square
Let u = u2 so the differentiai of u is bu = 2. u. 6~. In a DDA form this can be implemented
as in figure 3.6 except for the 2 multiplier constant, so figure 3.6 actually implements
1
Figure 3.6: This is the DDA implementation of the dinerential of the square of a function.
3.6 Error Analysis
This section introduces some basic concepts that are applicable to any process of A/D
conversion and later some of these concepts are applied to the DDA impiementations.
3.6.1 Basic Concepts
Quant ization Error
The general process of A/ D convertion is basicdly a three step process consisting of
Sampling, Quantization and Coding. If the sampling theorem is respected, in other
words, the maximum frequency of the analog system is less than the half of the sampling
frequency, then the sampling process is reversible without loss of information. The
quantization process is irreversible because an error is introduced. The quantization
error or quantitats'on noise
where eq(n) is the quantization emor, x,(n) is the quantized value of x, and r(n) is the
sampled value of the analog \&able of interest.
If 2,k and x,,,, represent the minimum and maximum values of z(n), and L is the
number of quantization levels, then the quantizatzon level A can be define as
There are basically two methods to discord the excess digits from the sampled
value, which theoretically has infinite precision, truncataon where the excess digits are
discarded, and roundkg where the number is rounded to the nearest value, which means
that Iz(n) - x,(n)l 5 4.
From the definition of 4 the quantization level, the range of the quantization error
in rounding is
Signal-to-Quantization Noise Ratio (SQNR)
The quality of the output of an A/'D converter is usually measured by the signal-
to-quantizution noise ratio (SQNR), which i s the ratio between the signal power and
the noise power. To evaluate this parameter, let the signal be a sinusoid such as
CHAPTER 3. DIGITAL DIFFERENTlAL ANALYZERS (DDA) 28
za = AcosRot. If the sampling theorem is satisfied, the only source of error is the
quantization error. To evaluate the quantization error the analog signal x, (t ) is quan-
tized instead of the discrete-the signal x(n) = x, (nT).
Suppose the quantization intervals are small enough to approximate z, (t ) as a linear
function between two quantization levels, so the quantization error will be:
where eq (t) is the quantization error and z, ( t ) is the quantized value of za (t). Let r,
be the interval x, ( t ) stays between to quantization levels 4.
Since the quantization process is rounding, the quantization error is:
The mean-square error power P, is:
With equations 3.28 and 3.29:
If the quantizer has b bits of accuracy and the quantizer covers the entire range 2rlt
CWT E R 3. DIGITAL DIFFERENTIAL ANALYZERS (DDA)
2A
the quantization step is A = -. Hence:
26
The average power of z. (t) is
From 3.31 and 3.32
Expressed in decibels (dB), the SQNR is
SQNR = 10 loglo SQNR = 1.76 + 6.026 (3.34)
Equation 3.34 implies that the SQNR increases approxhately 6 dB for every bit
added to the word length, that is, for each doubling of the quantization levels [Proakis 961.
For a generic signal, if the quantization error is uniformly distrbuted in the range
(-$,$) the mean value of the error is zero. In reference [Proakis 961 it is shown that
the variance in dB is:
R P
SQNR = 6.02b + 16.81 - 20 log - dB
0,
where R, is the range of the A/D converter and oz is the standard deviation of the
CHAPTER 3. DIGITAL DIFFERENTLAL ANALYZERS (DDA)
signal. The equation 3.35 is used to specie the precision needed in an A/D converter.
Due to limitations in the fabrication of .4/D converters, the performance falls short
of the theoretical value given by equation 3.35. The effective number of bits may be
somewhat less than the number of bits in the A/D converter.
3.6.2 Errors in the DDA
The fint error to be considered is the effect of the conversion fiom analog to digital
representation of the input signal. Because of the hi t e step size of the converter,
the digital number which is in Y., where the index n means the nth iteration of the
integrator. differs fiom the analog variable y, by eV, the round-off. This may be
represented by the equation
As descnbed in section 3.3, afier n steps of integrations, at the beginning of the
(n+ 1)th step, the converter is sampled and its output Yn is inserted into the Y register
of the first integrator. Y, is then added to 4: the d u e in register R, and changes it
to An overflow 6 2 is then generated and added to the integrand of the second
integrator, and changes its contents from Zn to Zn+i.
To find out the relation between the overflow bZ and the values in the registers Y
and R in the fkst integrator it s necessary to define the capacity of the R register. Let
M units be the capacity of R, which means that if hf units are added to the register R
an overflow is produced. When Y, is added into the R register three possibilities may
occuf:
CHAPTER 3. DIGITAL DIFFERENTIAL ANALYZERS (DD-4)
1. zero overflow, in which case: bZ = O and &+l = R, + Yn
2. positive overflow, in which case: bZ = 1 and = R, + Yn - M
3. negative overfiow, in which case: bZ = -1 and &il = & + Y, + M
Reference [Sizer 681 presents a formula where these three possibilities are combined
Summing the equation 3.37 from n = O to n = iV - 1, where N is the total number
of steps, gives
introducing the round-off effect into equation 3.38, as presented in equation 3.36,
gives
As was presented in section 3.3, the integration is the sum of the areas of the
rectangles of hight Y and width bX, which is an approximation of the area under
the curve. A better approximation for the area under the cuve is obtained using the
trapezoidal integation, where the area under the curve between two consecutive values
CHAPTER 3. DIGIT.4L DIFFERENTIAL AN.4LYZERS (DDA)
of y is
Substituting equation 3.41 into 3.39 gives
Equation 3.42 gives the relation between the values of the register Z and the ap-
proximated area under the curve and shows the various errors involved. The first point
I
to note is the introduction of the scale factor - which makes the equation non-
ki.6X
dimensional.
1
The term - (yN - go) is due to the sampng of the curve at discrete instants of
2 .bf
t h e and is the quantization error. As presented in reference [Sizer 681, this term does
not represent the complete error but it is the moa significant component. It is in fact
the contribution of the term (&Y)* in the Taylor expansion of the integrai. In the worst
case y~ = M and y0 = -M and the magnitude of this term will be one bit. Thus for
CHAPTER 3. DIGITAL DIFFERENTIAL .4NALYZERS (DDA)
simple integration this error will never exceed one unit in register Y.
The term
(R. - RN)
is the first rounding enor and it arises from the number in
-44
the R register of the first integrator. -2s the maximum value of the register R is MT the
maximum value for this error is one unit.
The term
(eqo + eql + - + e , ~- l )
M
is due to the rounding effect of the .4/D con-
- -
verter and it differs fiom the term
(al - RN)
M
in that it depends on the number of
integation steps taken. Since the average value of the terms e, is one-half unit, this
term contributes at the rate of one-half bit in M iterations steps.
Reference [Sizer 681 presents other error analysis for Mplementations of DDA for
multiplication, square, and exponential function.
In order to correctly evaluate the error through the DD.4 implemented for a par-
ticular problem, an andysis similar t o this one, for simple integration, should be done,
but for al1 the integrators in the solution model.
3.7 Sample Data Theory Applied to DDA
Some Elementary Discrete-Time Funct ions
1. Unzt Sample Sequence
2. Unit Step
1, f orn2O
0, for n < 0
3. Unit Ramp
n? f orn2O
u, (n) =
0, for n < 0
x (n) = an: for al1 n
Causal and Noncausal Systems
From [Proakis 961, a system is said to be causal if the output of the system at any
time depends only on present and past inputs. In mathematical terms, the output of a
causal system satisfis an equation of the f om
where F is an arbitrary function.
If a system does not satisfy this definition, it is cailed noncausal. Such a system
has an output that is dependent on the present, past and future inputs. The future
values of a signal are not observable in real-time signal processing applications, hence
CHAPTER 3. DIGITAL DIFFERENTL4L ANALYZERS (DD.4)
a noncausal system is physically unrealizable.
Given a discrete function z (n) the 2-Transform is defined as the power series
X ( z ) = C z (n) . t -"
Since the z-transform is an infinite power series, it exists only for those values of z
for which this series converges. The region of convergence (ROC) of X (z) is the set of
dl d u e s of t for which X ( 2) has a bi t e value.
I Simal. x (n) I z-Transform I ROC I
Table 3.3: 2-transform of common functions [Pr& 961.
( Q " c o s ~ o ~ ) ~ ( ~ )
A11 the properties and 2-transforms in tables 3.3 and 3.4 can be found in reference
[Proakis 961.
l - ~ z - ~ COSW
I- 2at - ~nrruni ~2r- 2
a z ' ' sin w
Defining the operator backward diffeerence v as:
121 > 1' 1
(an S" u (4 - 1 - Izl > lal
so from the time shifting property presented in table 3.4
Table 3.4: Propenies of the 2- t r dor m of comrnon funions [Proakis 961.
Property ) TheDomain 1 z-Domain ROC
1
3.7.3 The Integrator
Linearity
Time shifting
Scang
Time reversal
Backward dBerence
Assuming a sequential DDA and defining al1 functions in relation to a discrete iteration
cycle beginning at the first integrator, an expression for the output of the integrator may
be written dom at the nth iteration using backward dinerence operators, as proposed
in [Sizer 681.
In section 3.3 the integrator has been defmed as
To make it easier to understand the integration process a change in the notation is
necessary. Let P = O, +r, where the index n means the nth iteration, O, is the output,
At l em the intersection 1
of ROC, and ROC2
That of X (2) , except z = O
i f k >Oa n d z =o o i f k <O
laIr2 < 14 < 1 4 ~ 2
$ < 14 < &
That of X (2) , except z = O
i f k >( ) a n d r =o c i f k <( )
alxl in) + a2x2 (n) ' MI + a2x2 ( 2)
x (n - k)
anz (n)
2 (-4
Vz (4
Z - ~ X ( z)
X (a-%)
X (2-l)
( l - z - L) X( z )
CHAPTER 3. DIGITAL DIFFERENTIAL ANALYZERS (DDA) 37
and r, the roundsff. Let Y' = in where in is the input and bX = tn. Using the v
operator the 'integration' could be represented by
the scale factors are implicit in the ~ariables t, and r,.
Taking the 2-transform of equation 3.49
where the capital Z means the 2-transfom.
Finding the inverse of the Ztransform
Chapter 4
HRU Algorit hms Implemented in
DDA Form
This chapter will describe the algorithms implemented with the DDA technology for
the HRL System.
Work [Bolduc 951 is based on a 21 bit digitizer and a procedural software imple-
mented using 2 CPU's, one CPC for the I/O (inpuq'output) scheduling, tirner inter-
faces and processing interrupts, and the other CPC to calculate the navigation solution.
In this configuration the compensation is performed by the fim CPU at a faster rate:
while the navigation dgonthm is run at a slower rate due to the compiexity associated
with it.
Due to the tirne response of the digitizer and the required frequency of 200H2, the
precision of 21 bits could not be reached, instead an effective 17 bits was the precision
of the system [Hartl-1 971.
In order to solve these problems, this work provides a different approach with the
CHAPTER 4. HRU ALGORITHMS IMPLEMENTED IN DDX FORM 39
DDA technology. Under this approach, 2 CPU7s are no longer needed and it can
be implemented in by FPGA (Field-Programmable Gate Array) or EPLD (Erasable
Programmable Logic Device), reducing costs and improving reliability, since the number
of components are reduced.
The algorithm implemented is the same as that presented in [Bolduc 951. The
next sections begins by describing the algorithm and then the corresponding DDA
im plement ation.
4.1 HRU Alignment Scheme
Alignment for an inertial navigation systems (INS) is the process of determining the
angular relationship between the platform and the computational frame [Britting 711.
Extemal techniques such as optical alignment can be used for Wtem alignment. but
in cases like in a submarine this could not be a feasible method.
h self-alignment technique addresses the questions of how to use the outputs of the
inertial sensors, which are resolved in the platform kame: to determine the relation
between the platform frame and some reference f i e . The reference frame that is of
interest is the local-level wander-azimuth frame of reference.
[Britting 711 presents in detail gyrocompass alignment and analytic alignment. The
alignment scheme presented in reference [Bolduc 951 is divided into a two phase scheme
the coarse and the fine alignment.
The first phase is similar to the analyticai phase described in [Britting 711: with
a large sampling period to minimize the erron due to the sensors turn-on to turn-on
bias repeatability, instruments non-orthogonality, and external perturbations- such as
CHAPTER 4. HRI; ALGORITHMS IMPLEMENTED IN DD.4 FORM
vibration or accelerations, which can affect the graiity and earth measurements.
Since the approach presented in this work is based on DDA, the h t stage of corne
alignment was not implemented, but just the fine alignment stage.
The fine alignment stage consists of a Schuler-tuned filter used to correct the srnail
angle misalignments between the indicated computational frame of reference and the
true fiame of reference.
To explain the concept of the Schuler filter consider a simple pendulum suspended
over the Earth with pivot on the vehicle. When the vehicle is at rest the pendulum
axis is coincident with the true vertical direction, but when the vehicle is accelerated
the pendulum becomes aligned with the apparent vertical axis, which is offset from
the true vertical auis. -4 pendulum that remains aligned with the true vertical axis
in the presence of acceleration would have its pivot at the EarthYs surface and the
center of mass at the center of the Earth. The frequency of such a pendulum is the
Schuler frequency. This pendulum is physically unrealizable, by tuning a pendulum
at the Schuler frequency the angular velocity of the pendulum will be equal to the
angular velocity of the vertical. So the pendulum remains aligned with the vertical in
the presence of accelerations, and initial misalignments oscillate about the vertical with
a period of 84.4 minutes, the Schuler frequency.
In a Schuler-tuned mechanization, the accelerometer outputs are integrated and
divided by the earth's radius to detennine the rate to correct for linear motions. The
1
integrator can be replaced by a low pass filter with gain - With this approach R can
R.
be adjusted until optimum damping is achieved.
[Bolduc 951 and [Staley 841 presented the model in figure 4.1? the damped piatform
dignment model for the North axis accelerometer.
CHAPTER 4. HRU ALGORITHMS IMPLEMENTED IN DDA FORM
Figure 4.1: Damped Platfom Alignment Model for the Xorth-Axis .iccelerometer.
It is presented in [Bolduc 951 that this approach does not require the vehicle to be
stationary, which is advantageous because it does not need the user to wait for long
periods of tirne before departure, and allows alignment in a ship at sea subjected to
rolling and pitching motions.
In this approach navigation and fine alignment are concurrent whether or not the
vehicle is in motion, improving the navigation solution as gradually as the alignment
progresses.
In the following sections the various parts of the algorithm developed by [Bolduc 951
will be presented and how they are implernented in a DDA form will be described.
CHAPTER 4. HRU ALGORISHMS IMPLEMENTED IN DDA FORM 42
4.2 Compensation algorithms
The compensation of raw data fiom the accelerometer and nom the gyros can be found
from equations 4.1 to 4.3.
Gy= G 1 , GKz O O
O GK, O
Gt.1 GYZI O O GKz
[El] -
.
MU, .acz
MU, *acy
MD; .aczi
[ c i ;]
A, A,,
CHAPTER 4. HRIi ALGORITHMS IMPLEMENTED IN DDA FORM
where:
air
ai
aci
Ai
ABi
A Ki
G,
GBi
GK,
MU.
Qi
U
Ur
is the raw data from the accelerorneters for the 3'' axis;
is the compensated acceleration for the Ti" axis;
is the acceleration dong the gyroscope axis 'Y';
is the accelerometer misalignment angles, the fim subscript refers to the
body axis and the second refers to the system axis;
is the accelerometer bias drift for the 'Y' axis;
is the accelerometers scale factor, where i = {x, y, 2);
is the gyroscope misaligrnent angles, the first subscript refers to the body
axis and the second refers to the system axis;
is the gyroscope bias drift for the '5" axis;
is the gyroscope scale factors, where i = { x, y, 2);
is the gyroscope ma s unbalance drift term for the "i kxi s; and
is the gyroscope quadrature cirift term for the axis;
Compensated angular velocity;
Raw data from the gyroscopes.
CHAPTER 4. HRU ALGOMTHMS IMPLEMENTED IN DDA FORM 44
These equations are implernented in a DDA f om in the following sections. These
equations axe applied to the raw data from the sensors before alignment and navigation
algorit h m.
-4s can be seen in figures 4.2 and 1.3 this part of the algorithm consists of constant
multipliers and the bias are part of the initial values of the integrators. Special attention
should be paid, since the values are propagated through aii the others integrators.
4.2.1 Acceleration compensator
From equation 4.2, the compensator of the raw acceleration is found to be
a, = AK,.a, +AB, +A, ( AK, . +- AB, ) iA,(AK,.a,, - AB,)
~y
= Am (AKZ.am + ABz) + AK,.a, - -48, + A,, (AK,.a,, - ABz)
(4.1)
a, = A, (AK,.o, + AB,) + A, (AK, . a, - AB,) + .4Kz.a, - ABz
The integator scale factors of the integrators in figure 4.2 are presented in a table
form in table 4.1 where:
a ?VIk is the supremum of the accelerometers scale factors, iV& = sup(.4A,). Since
-1 5 Aij 5 1, Mk is also the supremum of the Aq.AKi terms, where i, j = x, y, z;
!Va is the size of the registers Y and R of the integrators, for a 21 bit register it
will be Na = 221 - - 2,097,152;
a ha is the maximum value that any acceleration can assume, so Ka = - is
N a
the integrator scale factor of the compensated acceleration ;
Figure 4.2: The DDA implementation of the equation 4.4, the compensation of the accelerometer
raw data where the bias, misalignments and scaie factors are applied.
CHAPTER 4. HRU ALGORITHMS IMPLEMENTED LN DDA FORM
~~
a K., is the scale factor for the accelerometer's output, K., = - where a, ,
N a
is the maximum output from the accelerometers;
Kt is the time base scde factor. It relates the time of the integrators and the
time base of the system.
The relation between the integrators {1,2,3) and 10 gives
= = h.i2 = &.
The same relation can be applied to the integrators {4,5,6) and 11. {7,8,9) and 12.
With these relations the table 4.1 has been built.
Table 4.1: These are the relations between al1 the scale factors of the integrators of figure 4.2. the
acceleration compensator.
The maximum acceleration output from the compensator. based on the integrator's
relations is
CHAPTER 4. HRu' ALGORITHMS IMPLEMENTED IN DDA FORM
4.2 -2 Angular Velocity Compensator
From equations 4.1, the compensation of the angular velocity component w, is
but from equation 1.3
From equations 4.7 and 4.8
CHAPTER 4. HRU ALGORITHMS IMPLEMENTED IN DD-4 FORM
With the same approach, but for the components wy and w, the following equations
can be found
wv = G,GK,w, + GKywyr + Gz2yGKzw,,
+ (Gzy G& + - GzlyGBz2)
+ az [G, (QzGziz + MU*) + Q&tk + iMUyGy=
- Gi2y (Qz~Ggz + MU&z2+)]
+ ay [G, (Q=G=iy + MII'G,) + QyG.2y + Ml&,
- Gzzy (Qr2 + MUz&z5)]
+
a: [Gzp (Qz + -vU'Gzzi) + Qu + MU9G9g2
- Gz2@ (Qz2G9z2 +
CHAPTER 4. KRU ALGOFUTHMS IMPLEMENTED IN DDA FORM
The equations 4.9! 4.10 and 4.11 have the form of
where i = x? y, z.
Based on equations 4.9, 4.10, 4.11 and 4.12, the compensator algorithm implemen-
tation using DD.. is presented in figure 4.3.
With the same approach of section 4.2.1, but with respect to figure 4.3, the cor-
responding integrator scde factors are presented in table 4.3. The parameters are
presented in table 4.2.
Figure 4.3: The DDA implementation of equation 4.9, the anguiar vdocity compensator for r ~ ,
component. The impiementation of the components w, and w, compensators are analogous to this.
CHAPTER 4. HRU ALGORITHMS IMPLEMENTED 13 DDA FORM
I Parameter I Value I I Parameter 1 Value I
Table 4.2: Parameters for the angular veiocity compensator in figure 4.3 based on equations 1.9.4.10
and 4.11 used in table 4.3.
CHAPTER 4. HRU ALGOFUTHMS IMPLEMENTED IN DDA FORiM 52
From figure 4.3, the integratoa {1, 2, 3, 4, 5, 6) and 19 have the following relations
Based on these relations and defining:
kIkW as the supremum of
K,,~ , K,,~ , K,,~
from table 4.2, where i =
x: y, z:
iMko as the supremum of
Kari , KaVi ,
from table 4.2? where i = x, y! z;
Wmo+
a Km as the scale factor of the output of the gyros, Km = -;
?lu
Ka as the scale factor of the output from the acceleration compensator, from
equation 4.5 Ka =
1Wk *am= ,
:va
!Vu as the size of the integrators' registers Y and R;
a Kt as the time base scale factor. It relates the time of the integrators and the
time base of the system.
The table 4.3 has been built with the integrator scale factors of figure 1.3.
From equation 4.13
CHAPTER 4. HRU ALGOFUTHMS IMPLEMENTED IN DDA FORM
Table 4.3: S d e fanors of the integrators in figure 4.3, the DDA implementation of the angular
veiocity compensator. The parameter Mko is the supremum of the t e m K, presented in table 4.2,
and Mb is the supremum of the terms Kuij also presente in table 4.2, where i , j = z, y, z.
CHAPTER 4- HRU ALGORITHMS IMPLEMENTED IN DDA FORM 54
Defining w,, as the maximum value for angular velocity, it can be found from 4.14 to
be
4.3 Navigation Algorithm
The navigation algorithm is used to integrate the position and attitude of the HRL
system. It is not the scope of this work to deduce this al gor i t h, which is well presented
in the references [Britting 711, [Bolduc 951 and [Siouris 931. It will be presented in this
section the principles and equations needed to implement the navigation aigorithm in
the DDA approach.
This process is concurrent to the compensation process presented in section 1.2. The
dignment mechanization is implemented as part of the navigation algorithm through
the damped spatial rates, which implements the modified version of the Schuler filter,
as presented in references [Bolduc 951 and [Staley 841.
4.3.1 Damped Spatial Rates
Reference [Bolduc 951 develops an algorithm for he-alignment, similar to [Staley 841
approach, where a damping ratio is applied to the ?ionh and East axes, the level axes.
CHAPTER 4- KRU ALGORITHMS IMPLEMENTED IN DDA FOR!
To find these results, it has been considered that the roll and pitch error angles are
independent fiom the heading angle error, and that ail bias instabiiity is negligible,
since they are trandormed by the small angle misalignment.
Al1 the hypotheses assumed, their consequences, stability and error analysis are
presented in reference [Bolduc 951. Rom those results, the optimum value for the R
and r parameters are found to be R = 3000 km and r = 1080 S.
The heading errors are also damped, but, unlike the level axes, a precession rate is
not applied to the azimuth channel to correct for the vehicle's motion over the spherical
earth. To ensure that heading errors are stable a rate correction is applied to the
azimuth channel. [Bolduc 951 presents the stability analysis of the azimuth channel
which leads to the result presented in equation 4.16, and the threshold value for *a7 as
0.003 s/m, below which the system would become unstable for the case study of 4a0
latitude position. From experimental and simulation work the optimum d u e for 'at is
presented to be a = 0.43 s/m.
Following the approach in [Bolduc 951 and from equation 4.16, the damped spatial
angular rate between the computational kame of reference and the body frame of
reference is
The following relations are found kom integrators in figure 4.4 (a) and (b).
Figure 4.4: DDA implementaton of equations 4.16 and 4.17, the damped veiocities and angular rates.
This is the implementation of the self-alignment meehanization as proposed in references [Bolduc 951
and [Staiey 841
from integrator 1:
fkom integrator 2:
from integrator 3:
From the relations of the integrators 1, 2 and 3 in figure 4.4, the fouowing constraints
CHAPTER 4. HRU ALGORITHMS IMPLEMENTED IN DDA FOhM
are found
The integrator scale factors of the integrators in figure 4.4 are s how in table 4.4
where:
Kvc is the integrator scde factor of the crue velocity, that will be addressed in
section 4.3.7:
O v,, is the maximum value that Y cmassume ;
1
MT is the supremum of -;
7
I
0 MR is the supremum of - O
R:
Np is the size of the integrators' registers of the damped rate DD.4 implementation:
0 Kt is the time base scale factor, it relates the time of the integrators and the time
base of the system.
With respect to the integator in figure 4.4 (c) its scale factor is
CHAPTER 4. HRU ALGORITHMS IMPLEMENTED IN DDA FORM 59
Table 4.4: The integrator scale factors of the DDA implernentation of the damped velocities and
angular rates in figure 4.4, where i = {z, y} and j = {y,x). These relations can be found from the
conesponding imegrators as presented in section 3.4.3. The basic relations between the inputs and
Mx,
output for each integrators are K,, = - y Kyi = - -% and K, = I(.,.KPi.Xi = KZi. M, , where iM
Ni Ni
is the supremum of the conesponding input.
where KAc is the tnie acceleration integrator scale factor, which will be presented in
section 4.3.5.
-4s K, in figure 1.4 (c) should be equai to K,, = Kp in figure 4.4 (a) and (b) the
following relation is found
where Ma& is the supremum of the coefficient 'a' of equation 4.17.
4.3.2 The Direction Cosine Matrix Q
The DCM Q is the direction cosine matrix that transforms a vector in the system frarne
of reference to the computational hame of reference. It corresponds to a sequence of
roll rotation (@), followed by pitch rotation ( O) and yaw rotation ($). It can be found
from reference [Stdey 981 to be the following matrices as functions of the quaternions,
CHUTER 4. HRU ALGORITHMS IMPLEMENTED IN DDA FORM
equation 4.23, and of the Eler angles, equation 4.26
The Q matrix elements are implemented in DDA in figure 4.5 from the equation
4.25, where the inputs are the quaternions 6qi increments.
The integators in figure 4.5 (b), (c) and (d) have the same topology and can be
described through the same table of integrator scde factors. Table 4.5 represents the
integrators in 4.5 (b).
The integrators in 4.5 (a) correspond to the diagonal elements of Q and their inte-
grator scale factors are presented in table 4.6, where:
K, is the integator scale factor of the DDA implernentation of the quaternions
to be presented in section 1.3.6;
N, is the size of the integrators of the quaternions, it is also the size of the
integrators 1 to 12 in figure 4.5 (a), and 1 to 8 in figures 4.5 (b), (c) and (d);
0 NQ is the size of the integrators' registen 13 to 15 in figure 4.3 (a) and 9 and 10
in figures 4.5 (b), (c) and (d).
Figure 4.5: DDA hplementation of the DCM Q that transforms from the system lrame of reference
to the computational kame of reference.
CHAPTER 4. HRU ALGORITHMS LMPLEMENTED IN DDA FORM 62
From section 4.4, the supremum of the quaternions M, must be equal to 1. From the
integratoa 1 to 12 in figure 4.3 (a) and 1 to 8 in figures 4.5 (b) , (c) and (d), the size of
the registers must satisk the condition N 2 N, , in addition a constant multiplier of 2
must also be applied to bQ resulting in the condition NQ 2 N,.
From these tables Kq, the integator scale factors for the increments output of figure
4.& needs to correct the times 2 factor. Defining Mz 2 2 the value of KQ is
4.3.3 The Direction Cosine Matrix D
The direction cosine matrix (DCM) D transforms coordinates from the earth-&ued
frame of reference to the computational frame of reference. It is used to find the
components of 4, , the earth angular rate, in the computational frame of reference and
compensate it from the angular rates measured by the wo s .
[Bolduc 951 and [Siouris 931 present how one can find the following equation of D.
Defining as the angular rate of the computational frame of reference with respect
Table 4.5: Iutegrator s d e factors of the DDA implementation of the diagonal elunents of figure 4.5
(a), the DCM matrix that transforrns from the systern frame of reference to the computational fiame
o reference.
Table 4.6: Integrator scaie factors of the DDA implementation of the non-diagonal elemenrs of Q
presented in figure 4.5 (b), the DCM matrix that transfolpls fiom the system frarne of reference to the
computati~d frame O reference. These relations can be found from the corresponding integrators as
presented in section 3.4.3.
The basic rdation. between the inputs and output for each integratocs are Kzi = 2, K,, - -- -%
iq
and Ksi = K,, Gi .i& = K,, ..UV,, where M is the suptemum of the corresponding input.
to the earth-centered earth-fued frame of reference, is shown in [Siouris 931 that
where V, is the tme velocity of the vehicle and El, is the earth radius. As the wander-
azimuth is the mechanization used, it is assumed that j, = 0.
From the definition of @, the rate of change of the direction cosines for the wander-
azimuth mechanization, where fi, = O , is [Siouris 931
The sets of integrators (3, ..., IO), (11, ..., 18) and (19, ..., 26) have the same topol-
ogy, as a result, the table and relations of integrator scale factors are the same. With
this in mind only the table for the integratoa 1 to 10 is presented and the results can
be directly applied t O the ot her integrator sets.
CHAPTER 4. HRU ALGORITHMS IMPLEMENTED IN DDA FORA4
Figure 4.6: The DDA implementation of the DCM D that t d o n n s from eanh-centered eanh-fked
fiame of reference to the computational frame of refrence.
Assuming Kzi is equal to i = (3, ... , IO}, the foilowing relations are found
The integrator scale factors for the D elements can be found and are presented in table
Table 4.7: Integrator scde factors of the DDA implementation of the DCM D terms, the DCM that
transforms from earth-centered earth-ked frame of reference to the computational frame of reference.
These relations are found fiom the corresponding integrators as presented in section 3.4.3.
- Mvi
Kv, - - The basic relations between the inputs and output for each integrators are K,, = -
iq *Mi
and KZi = KXi.Kvi Ni = KZi .Mfui, where M is the supremum of the corresponding input.
From the relations presented in table 4.7, the scaie factor for the D, terms is
itegraton 1 Y Registers AX
mx
&VU
6D3,
&&
6
1
2
3
4
1
-
bu
D3 I
OY
-
.. -
ti&
6 0 3 1
JD31
1 5 D31
M
MR,
MR,
KV~.MR*.ND
ih
MD
K;r
Kve
Kve
MDIND
K v . M~
KVc . M~
1
K: 1 Ku
Kvc .iMR,
KV~*MR.
K V ~ . ~ I R ~ .J%
KvC.MR, .MD
KV~-LY& .MD
.UR, / ND
-&~JJVD
Kv c J~R,
MD /h
MDIND
CHAPTER 4. HRU ALGORITHMS IMPLEMENTED IN DDA FOM
t herefore
1
where & is the supremum of the DG terms of the DCM D, and hl, 1 - where R.
%'
is the earth radius.
In section 4.1 will be shown that fi = 1 as the supremum of the DCM Q terms.
4.3.4 n u e Angular Velocity
The tme angular velocity relative to the computational reference frame is found from
the DCY Q, the angular rates from the gyros in the body frame of reference. the
damped angular rates and the earth rate in the computational f r me R,
w here:
pc is the damped angular rate of the computational frame of reference with respect
to the ea,rth-centered eanh-fixed fiame of reference. In reference [Siouris 931 the
v,
not damped rate is also caiied transport rate, pz = =-
VZ
%:
& = - and ,6, = O : as
Ra
presented in section 4.3.3;
R, is the earth rate relative to the earth-centered inertial frame of reference trans-
formed to the computational frame of reference, Rc = D.R,,, where D is the DCM
that transforms from the earth-fixecl fiame of reference to the computational frame
of reference, as presented in section 4.3.3.
So the true angular velocity components relative to the computational frame of reference
are
Figure 4.7 presents the implementation of the true angular velocity components
relative to the computationd frame of reference in the DDA approach.
Figure 4.7: DDA implementation of equations 4.33, the true angular velocity components relative
to the cornputationai frame of reference.
The topology of the integrators in figure 4.7 (a), (b) and (c) are very similar to each
other, so the results presented in table 4.8, the integrator scale factors of figure 4.7 (a):
can also be applied to the integrators in figures 4.7 (b) and (c).
Table 4.8: kitegrator scaie factors of the DDA implementation of equations 1.33, the uue angular
velocity component po referenced to the computational fame of reference. These relations can be
found fiom the corresponding integrators as presented in section 3.4.3.
-G, .PIY,
The basic relations between the inputs and output for each integrators are Kz, = -
.!!
9 Ku, =
and K,, = Kxi*Kv, .h$ = K,, .Mvi, where M is the supremum of the corresponding input.
From the relation between the integrators
the following relations are found
where:
a KQ is the uitegrator scale factor of the DCM Q terms presented in section 4.3.2;
0 Ku is the integrator scale factor of the compensated angular velocity presented
in section 4.2.2;
0 Kp is the integrator scale factor of the damped angular rate presented in section
4.3.1.
4.3.5 nue Acceleration
This section will focus on the DDA integrator topology for the transformation of the
acceleration compensated AT = frorn the body frame of reference to
the computational frame of reference.
From section 4.3.2, Q is the DCM that transforms coordinates from body frame of
reference to computational h e of reference, so
Figure 4.8 shows the topology for the transformation. Table 4.9 presents the scaie
factors for the integrators of the component increment bA,. As dl three axes are
analogous, the results in table 1.9 can also be applied to the components &4, and
6Ae.
From table 4.9 the relation between the integrator sale factors of the true acceler-
ation components and the compensated acceleration components is
CHAPTER 4. HRU A4LGORITHMS IMPLEMENTED IN DDA FORM
Figure 4.8: DD A implementation of equation 4.37 the true accderation components in the compu-
tational frarne of reference.
Table 4.9: The acceleration integrator scale factor rdations for the x-axis component. F'rom figure
4.8 these relations cm be found from the corresponding integrators as presented in section 3.4.3.
-Mv,
K,, = -
The basic reiations between the inputs and output for each integrators are KZi = -
Ni -Xi
and K,, = Kt, Gi Ni = K,, .Mu,, where M is the supmum of the corresponding input.
CHAPTER 4. HRU ALGORITHMS IMPLEMENTED LW DDA FORM
where:
KQ is the integrator scale factor of the DCM Q terms presented in section 4.3.2;
0 Ka is the integrator scale factor of the compensated acceleration presented in
section 4.2.1.
4.3.6 Quaternions
Reference [Staley 981 shows that the values of the quaternion derivatives can be obtained
from the 4 x 4 skew-syuimetric matrix, as in equation 4.39
d
Taking the term - (9,)
dt
Approximating the differentials dqi and dt to 6q, and 6t and applying it to a DDA
integrator as in figure 4.9(a), the term q,.6t will be the output 62. With this in mind.
using qi6t the 62 input and bpj as the by input, the terms pj- (qi.6t) are obtained
as the output of the DDA integrator presented in figure 4.40(b), where j = z, y, r and
i = 1,2,3,4, .
Figure 4.9: Simple DDA integrator of a term with output equal to qi.6t as in equation 4.40.
Applying the integraton in figure 4.9, the DDA implementation of equation 1.39 is
obtained as shown in figure 4.10, where:
a M, is the supremum of qi the quaternions, with i = 1, 2 and3;
1 1
is the supremurn of the constant multiplier factor - LW& >_ - *
2: 2
a ;Vq is the size of the registers Y and R of the integrators;
a from section 1.3.4 equations 4.34 and 4.36 Kp = Kw.KQ.?l, = Kp.
The four topologies conesponding to ql , qz, q1 and q4 are very similar, varying just
in the signal of some multiplies output, which can be achieved changing the signal of
the increments. With this in mind, table 4.10 is presented with the integrator scale
factors for the ql implernentation. Figure 4.10 (a), and its results can be applied to al1
the othea quaternions implementations.
Table 4.10: Integrator scale factors of the DD.4 implementation of the quaternions that relates the
computational trame of reference and the systern frame of reference presented in equation 4.39.
4Y
6q2
3
Integrators / Y Registers 1 4X
KY
Mq /ATq
KP ,
MQ/&
M 1 KI 1 K:
.kfq
K,.&
Mq
dt
q2.&t
'ft
1
2
3
q3-6t
'ft
q4Jt
92
P=
93
4
L ,
&/-% ,
KP ,
iWaq/Nq
644, 1 K w - &
6th 1 Mq
6 ~ s 1 KM-&
- 1 Mao
7 I 1/2 [ 2.dqi
Pu
Kt 1 KtJ&
5 I 94
6 I Pz
Kt .M,
Kfi
Kt .Kw .Adq ..!!!Yq
Kt .Mq
Kt. -&
KR
Kt &
Kt.K, .M,.N,
&Mq
Kt. K, . Mq . Nq
Kt.K,.Mq.Nq 1 Map .Kt .KM .Mq .NP
CHAPTER 4. HRU ALGORITHMS IMPLEMENTED IN DDA FORU
Figure 4.10: Quaternions' ciifkentia1 equation 4.39 implemented in a DDA fom using the inregrators
presented in figure 4.9. These are the quaternions that relates the computationd frarne of reference
and the system fiame of reference-
CHAPTER 4. HRU fiGORITHMS IMPLEMENTED IN DDA FORiM 75
Rom the relations between the integratm
4.3.7 True Velocity
The following equations of the true velocities relative to the computational b e of
reference can be found in [Bolduc 951 and [Siouris 931.
Assuming the simple gravitational mode1 presented in [Siouris 931, equation 4.42 is
simplified to
and is impiemented in a DDA form as in figure 4.11.
Since the topologies of the DDA implementations of the components of the true
velocity V, are very similar, the results found for the component 6V, in table 4.11 c m
also be applied to the components 6Vw and 6V,, where:
hf& 2 2.0,: where Qk is the earth rate;
Figure 4.11: T h e veloty differential equations implementation in a DDA form.
Mvc is the supremum of the true velocities that the system can measure;
Mvc is the supremum of the derivative of the true velocities;
0 Mm is the supremum of the terms (2.Q.Dij +a), k = {x, y) and 2, j = {1, 2. 3).
From table 4.3 .MPD = KD.~&n.Nvc = K,.LV,~;
O Xvc is the size of the registers Y and R of the integrators.
Table 4.11: Integrator scale factors of the DDA implementation of the component dV, of the
true velocity , figure 4.1 1 (a).
The following relations between the integator scale factors can be found from figure
4.11(a).
from integrator 1:
O from integrator 2:
from integrators 3 and 4:
O from integrators 5 and 6:
O fiom integrator 7:
a from integrator 8:
0 from integrator 9:
from equations 4.44, 4.43, 4.46, 4.47, 4.48, 4.49, 4.50 and table 4.11
Following the approach presented in [Bolduc 951, the simple gravi tational mode1 has
been applied which resulted in the initial value of integrator 7 t o be g, = -9.806 scaled
Kvc
by K,, = - from figure 4.11 (c).
Nvc
4.4 Ort hogonalization
In sections 1.3.6,4.3.2 and 1.3.3 the results must be orthogonalized in order to minimize
error propagation. As has been done in the previous sections, the sarne approach that
was applied in [Bolduc 951 will be implemented in a DDA form in the following sections.
CHAPTER 4. HRU ALGORITHMS IMPLEMENTED IN DD.4 FOhV
4.4.1 Quaternions
To orthogonalize the quaternions let
so the onhogonalized quaternion is
Assuming c < 1 and taking the first t em of the Taylor expansion
finding the differential of
and the differentiai of
With the equations 4.56 and 4.57. the DDA approach for the orthogonalization of
the quaternions is presented in figure 4.12.
From the relations between the integrator scale factors shown in table 4.12 the
Figure 4.12: DDA implementation of the orthogonaiization of the quaternions based on the Taylor
expansion of the error c of the orthogonalization, as presented in equation 4.37.
Table 4.12: Inregrator scaie factors of the DDA implementation of the onhogonalization of the
quaternions, as presented in figure 4.12, based on the Taylor expansion presented in equations 4.56
and 4.57.
CHAPTER 4. HRU ALGORITHMS IMPLEMENTED LN DD-4 FORM
foilowing relation must be satisfied
the constraints that follow are found to be
4.4.2 Direction Cosine Matrices
D and Q are DCM that transform coordinates from the earth-centered earth-ued
frame of reference to the computational frame of reference and from the system frame
of reference to the navigational frame of reference, respectively. -411DCM matrices are
orthogonal and so their determinants must be equal to 1. The determinant of the DCM
D can be approximated using the same approach presented in the previous section:
So the normalized element of the DCM D is
..\ssuming c < 1 and taking the first term of the Taylor expansion, the nomalized
element of DCM D can be approximated to
With the same approach presented to the quaternions, but now for each element of
the DCM
6~~~ 6 [Di,. (1 - e)]
wbere i, j = 1: 2,3.
As was presented in diaprer 3.4 for the DDX implementation of the product of two
functions, the differentiai of each function is needed. In equation 4.60 6Dij is known.
Figure 4-13 (a) is the implementation of k. The nomalized terms of the DCM are
found by applying the multiplier DDA presented in figure 4.13 (b) where i, j = 1,2? 3.
The same results are valid for the Q DCM matrix, so they are generically applied
to every DCM.
Al1 the integrators are products of 3 dinerential terms. Table 1.13 presents the
integator scaie factors for the f ht set of muhipliers, the result s applicable to al1 the
others since they have the same topology.
Figure 4.13: DDA implementation of the enor term 6t of the orthogonalization of the DCbI elements
as presented in equation 3.61. In this implementation only the first order term of the Taylor expansion
has been taken into account.
Table 4.13: Integrator scale factors of the DDA implementation of the orthogoaaiizatioa of the
elements of the DCM as presented in figure 4.13 based on the Taylor expansion presented in equation
4.60.
In figure 4.13 (b) the integator scale factor is
The scale factor of 6Dij must be equal to the scale factor of 6Dij which implies
the constraint that follows are found to be
4.5 Constraints
Sections 4.2.1 to 4.4 presented the DDA implementations for the compensators, align-
ment and navigation aigorithms and in each one some constraints were found. Nom,
in this section, dl these constraints are put together in order to reach more general
constraints applied to the system as a whole. -411 constrahts found are listed in table
I Section 1 Const raints I
Table 4.14: This table summarizes ail the cornrainu found nom the relations between the integra-
ton presented in the previous sections. They have be n implemented in the sofrware as pan of the
initiakation process or as a consrraint verification for each integration step.
From sections 4.3.2, 4.3.4 and 4.3.6
CHAPTER 4. HRU ALGORJTXMS IMPLEMENTED IN DDA FORM
From section 4.3.2
*bIo
Kq = M2.Kq.Mq a -
Mq M
- M2.-.r
-% iV,
but fiom section 4.4
As !Vq and .?Vq are powers of 2, 16 is chosen to be M2 = 2 2 2. which results in
NP = 2.?Cfq.
With equations 1.63 and 1.64
From section 4.3.7 and 4-3.4
CHAPTER 4. HRU ALGORITHMS IMPLEMENTED IN DDA FOKM 88
but ftom section 4.4
with equations 4.63, 4.64 and section 4.1
From sections 4.3.7, 1.3.5 and 4.4
From sections 4.3.5, 4.3.1, 4.4 and equations 4.66, 4.67
CHMTER 4. HRU ALGORITHMS IMPLEMENTED LN DD.4 FORM 89
From sections 4.3.4, 4.3.1, 4.1
and from section 4.3.7 K, = KD.M2n so
From sections 43.4, 4.3.1, 1.4 and equations 4.63.1.66, 1.67, 4.69
CHAPTER 4. HRU ALGORITHMS IMPLEMENTED ZN DDA FORM
From equations 4.66, 4.67 and 4.70
From section 4.3.1 and equations 4.68 and 4.71
From sections 4.3.4, 4.3.7 and equations 4.63, 4.66 and 4.69
Also from section 4.3.4 and equations 4.72, 4.63
which gives the same resuit.
From section 4.3.7 and equations 4.66 ,4.70, 4.71
but also from section
which gives the sarne
4.3.3 and equation 1.70
To summarize the relations between the size of the integrators and the supremum
values of the integrators found in this section, the table 4.15 has been generated.
.Ni these relations are implemented in the DDA sohware as part of the initialization
CHAPTER 4. HRU ALGOFTHMS I Mf LEMENTED IN DDA FORM
L
1 KT i defined 1 1 Mm
F
defined
defined
de fined
Table 4.15: This table summarizes the relations presented in section 4.5, the relations between the
size of the registers and the supremum of the integrators in each of the algorithms' steps, These
relations are implemented in the software as constraints.
CH.4PTER 4. HRU ALGORITHMS IMPLEMENTED IN DDA FORM
process of each class and/or as a constra.int venfication step through the integrations.
Chapter 5
Software Implement at ion
The principal idea behind the use of a DDA is to implement the algorithms developed for
a particular problem in hardware, in an FPGA or EPLD for example. The algorithms
can then be run in parallel, improving performance. Since the hardware to implement
the DDA proposed in chapter 4 was not available, ail the algorithms presented in chapter
4 were implemented in a sequential approach using C t t .
This chapter will present the architecture of this implementation, the mrious classes,
attribut es, met hods, and interconnections.
In a hardware implementation, the overflow and the size of the registers are im-
plemented in the hardware. For the sequential approach, they are implemented as
comparisons and subtractions to generate the same effect. ..As a consequence of this
implementation the performance is drastically reduced. Each step in a parallel im-
plementation is quivalent to hundreds of comparisons, additions or subtractions in a
sequential implementation. .Uso there are the overheads to c d each integrator method.
With al1 these cornmands the one step in the parallel implementation is translated
CHAPTER 5. SOFTWARE IMPLEMENTATION 95
into thousands of instructions in a sequential form. Since the frequency needed is
approximately 580 kHz, the software approach (sequential organization) could not be
implemented in the HRU hardware.
-4s a consequence, ail results were found through the simulation software using the
same data files that Carole Bolduc used in [Bolduc 951.
The software has been developed under Linwc kemel version 2.2.14, using the GXU
C;+ compiler gcc 2.95.2, Python version 1.5.2 and Tk version 8.0. The Linux OS has
been chosen because of its stability and the tools available through the internet at very
low cost*
Al1 C i + code is C i l .LISI and can be ported to other operational systems with
no additional effort using a C i - ANSI cornpliant compiler.
The graphical user interface (GUI) was developed using Python, an object oriented
script language, which made it possible to develop the stripchans more easily than
under C++. It is a very good script language for fast prototyping, for laboratory
interface or GUI development. It is also portable to other operational systems, such as
Windows 9xm.
To develop the stripchart program, the libraries BLT version 2.4 and PMW version
0.8.3 were used under Python. BLT is a gaphic library to Tk and PMW (Python
Megawidgets) consists of a set of base classes that makes the Tk programming under
Python easier.
CHAPTER 5. SOFTWARE LMPLEMENTATION
5.1 Software Implementation
This section presents the description of the classes, their attributes, methods and how
they relate to each other.
Since the number of integrators needed to implement the navigation dgonthm is
too big, the size of the software made it impractical to be put as an appendiv to this
work, since it has almost 10.000 lines adding al1 files together.
The source files are:
i nt egr . cpp
dda-alg-cpp
comp. cpp
navi . cpp
nav2. cpp
navigat . cpp
or t ho. cpp
si mul l . cpp
interpol . cpp
tspl i ne .cpp
navs imul . cpp
mainnav . cpp
datagen . cpp
stri pchart. py
Each fiie has the correspondhg header file with the description, attributes, methods
and constructors for each class.
CHAPTER 5. SOFTWARE IMPLEMENTATION 97
Al1 algorithrns have been implemented as object orienteci classes that work as col-
lectors of integrators and inherit the cornmon behavior from the base class TDDA - Mg.
The navigation and compensation algorithm classes are implemented in the files comp. cpp,
navl . cpp, nav2 . cpp and navigat . cpp. The orthogonalization aigorithms are imple-
mented in the file ortho. cpp .
The class TSimull which is responsible for reading the configuration files, the
raw data and interface the navigation class T'r'avigation is implemented in the file
s imull . cpp.
Since the data files had been sampled with a l Hz sampng frequency, it was nec-
essary to interpolate the data points in order to generate the sampling frequency of
580,922 Hz. To interpolate the data points the Tinterpol class was used. which im-
plements a cubic B-Spline interpolation routine based in the algorithm proposed in
[William 981. It is implemented in the file tspline . cpp.
stripchart . p y is the python script that generates the stripchan of the navigation
program as the simulation is run, enabling the user to see the behavior of the navigation
variables quaternions, DCM D and Q, true angular velocity pCt damped angular rates
p, true acceleration A, and true velocity V,.
This script file has two configuration files stripgraph. conf and stripchart . conf.
The atripgaph. conf defines the size of the graphs and font to be used: enabling
st r i pchar t . py t o run in different resolutions. The stripchart . conf has information
about the variables being plotted, such as the titles, the range of the x-axis t o be
displayed and the amount of it to be shifted.
Appendix B shows the description of the files that are part of the simulation sohware
navs imul*
CHAPTER 5. SOFTWARE IMPLEMENTATION
5.2 Configuration Files
This section presents dl the configuration files used by the simulation programs. The
content of each one is presented in appendix C.
hni. cfg this file has ail the parameters for the navigation and is loaded by the TSirnul
instance.
inout. cfg this file has the directories and names of the input and output files.
datagen . cf g this file defines the parameters for the datagen program.
stripchart . conf this file has di the parameters of the python script stripchart . py,
such as the title of the graphs, number of iterations per point, etc.
stripgraph. conf this file has the information needed by stripchart .py to create the
output windows at different resoiutions. There are two versions of this file one for
a 640x180 and an other for a 1200x768 graphitai resolutions.
optionil0 this file has the font to be used by stripchart . py.
Chapter 6
Results
The DDA approach is intended to be implemented in hardware allowing the integrations
to be done in parallel through al1 the stages of the algorithm. Since the hardware
implementation was not possible, the sequential implemeritation using C- r i has been
done, as presented in chapter 5.
As a result of this approach, the software has 304 integrators implemented in 11
classes in the simulation software.
In reference [Bolduc 951 there are severai simulation data files, and several test
r us developed with the same algorithm that is implemented in this work, but the
sampling frequency was 1Hz and the frequency needed for a resolution of Plbits in the
DDA implementation is 580,923H2, as presented in table 6.1, the parameters file of
navs imul.
So it was necessary to interpolate 580,923 points for each point in the data files,
which was done using the classes TSpline, TXntAcc and TIntGyr presented in chapter
o.
CHAPTER 6- RESULTS 100
The use of the B-Spline interpolation plus the nwnber of integrators have generated
a great overhead for each iteration, and because of this the simulation time has become
very long. To simulate a file with 510 points it takes approximately 5 hours with an
AMD KG-II at 300MHz with 64Mbytes of RAM, and 310 points means 8.5 minutes
with a sampling frequency of 1Hz.
According to reference [Bolduc 951 the tirne needed to damp haif of the drift in pitch,
for example, would be about 101800 seconds, which corresponds to approximately 106
houa, or 4.4 days, of simulation with this sequential implementation.
So because of the r e dy long simulation time, the results presented in this chapter do
not have d l sets of simulations as presented in reference [Bolduc 951. The two that were
considered relevant to veri& the results with this implementation are shown bellow.
6.1 Simulation Results
This section will present the results of two simulations. The first one is the simple
simulation of an ideal stationary vehicle, to venfy that the output is stationary as it
should be*
The second simulation is with a red stationary input from a sample file from the
HRQ used in [Bolduc 951 for the coarse alignment test and for the anaiysis of the
damping parameters T aad R. The use of the real sampied data with its inherent noise
and bias drift is important to venfy the stability of the estem.
CHAPTER 6. RESULTS
6.1.1 Ideal Sample Data
The ideal sample data file was generated with the datagen program. This program
generates a file with the same format of HRU's, with the corresponding bias, quadrature
and mass unbalance terms of the sensors, applied for a chosen position and attitude.
The sample data was generated for:
0 Latitude 48'
Longitude -123O
0 wander O0
roll O0
pitch O0
yaw 0
The configuration file for the navs imul program is presented in table 6.1.
Since for 200 points the generated interpolated data would have 116,184,600 points,
which corresponds to a file with approximately 3 Gbytes, for a double precision in-
formation in a binary form, the output of the navsimul was generated in the same
frequency of the input file, at 1Hz. At ewry new data point the most recent status of
the integators was transformed into position and attitude information and was written
in the output file. Dunng aJl simulations the mean values of attitude and position were
also calculated and written to the mean output file at the end of the simulation.
The mean values of position and attitude fkom the navsimu progam are:
Wander =O .O00000
Latitude =47.999851
CHAPTER 6. RESULTS
Longitude =57.000075
Yau =O. O00000
Pitch =O. O00000
Rol l =O 000000
To follow the simulation software behavior the stripchart-py script rias used. It
presents the status of the simulation software parameters corrected by its integrator
scde factors in stripcharts. Figure 6.1 presents the plots generated by this program
during the simulation. It does not present the stripcharts of dl the simulations because
of the reaons presented above.
As can be seen in figure 6.1 the expected constant output was generated.
6.1.2 Static Vehicle Sample Data
This simulation used the files hm-acc3. out and hru-gyr3. out, the same files used
in [Bolduc 95) for the coarse alignment test. These files were generated at the DREO
facilities with the HRU local-leveled and pointing north with the senson oriented as
follows:
Gyro SIN 19: X-Case: East Y-Case: Down
GJTO SIN 24: X-Case: North Y-Case: Down
.4ccelerometers: -4.X (S /N 232) : East
.4Y (S/Y 228): South
AZ (S/N 234): Up
The configuration file and position settings are the same used in the ideal conditions
presented in section 6.1.1 and table 6.1.
CHAPTER 6. RESULTS
I 1
1 AB (g) 1 0.00292658083 0.0004389346 0.009026187
T 1080
R (4 3,000,000 m
% (4 6,378,140 rn
act 0.43
Table 6.1: Configuration file used by navsimul program to simulate the ideal input file generated
by datagen program in order ta wnfy the aatic vehide ideal input.
CHAPTER 6. RESLTS
Figure 6.1: St ri pdmt a of the bauiubn uaing the ata Bb geaer.tsd by datagan pogrm an the
idal input Bb ta pmgam nava-, in ada to the WC vehicie icieaJ input.
CHAPTER 6. RESULTS 105
The mean output values of position and attitude found through the navigation
program from [Bolduc 951 are:
Wander =O. O00000
Latitude =45.35004
Longitude =- 122.99785
Y au =O. 00438
Pitch =O. 09175
Roll =O. 10920
Following the same approach presented for the ideal data, the stripcharts in figure 6.2
were generated. In this set of stripcharts it can be seen that the bias in the accelerom-
eten are not cxactly equal to the value in hru.cfg which generated the velocity compo-
nent. The same can be verified with respect to the angular rates and the quaternions
values.
The mean values are as expected even with the bias drihs and noise in the sampled
data file. The results from [Bolduc 951 work are:
Wander =O. O0000
Latitude =45.34824
Longitude =-122.99990
Yaw =O. 00211
Pitch =O. 00264
Roll =O. 00209
Figure 6.2: Stripch.rtr of the dmuiaticm uning t&e data fiie umpied u lHz wnpling kqmqr fimau
HRU in D m iw?iiith with it ld-lavcted and pointing ncnth.
Chapter 7
Conclusion
The results presented in chapter 6 verified that the system is stable for a static input.
Ir is assumed from the results presented in [Bolduc 951 that it will work for normal
conditions, since the parameters used are the same.
As presented in chapter 6, the simulation time is too long and no other simulations
have been made, such as the damping of attitude errors or tracking position simulations.
The parameters of the integrators are strongly connected to each other? as presented
in section 4.5. The change in the value of one parameter impiies changes in the others
in order to keep the compatibility between the stages of i nt emi on.
The values presented in table 6.1 of chapter 6 were implemented with a spreadsheet
using StarOfficeTM version 5.1. As the changes in one parameter implies changes in
others, and with the necessity that Ni be a power of 2, this analysis was lengthy
The pr~cision of the system is related to the sampling frequency and the maximum
values of angular velocity, acceleration and velocit y expected. For different applications,
different values should be found-
The maximum parameters for the sensors, such as quadrature, mass unbaiance
and scale factors, also need to be in a range defined by the supremum values of the
integators. If new sensors have their parameters out of the range a new parameter
analysis hould be done.
Special care should be taken to define the initial values of the integraton and the
correct units of the values, since these values are propagated to the other integrators,
therefore affecting the behavior of the entire system.
The use of the stripchart.py program is a powefil tool to debug and diagnose
the system performance, as the saturation of registea and the propagation through the
integraton can be seen in real simulation time.
7.1 Future Research
As future research:
1. The error analysis of the DDA implementation of the navigation and compensa-
tion algorithms. This analysis has not been developed because of the complexity
as a consequence of the large number of integratoa, 304 integraton. Ln section
3.6 the error analysis for a single integrator was presented.
2. The implementation in hardware of the DDA presented in this work using devices
such as FPGA's, which can enable the performance analysis that could not be
developed using the simulation software.
3. Implementation of an optimization process to define the values of the 34 parame-
ters of the DDA implementation, since these values are dependent on each other.
CHAPTER 7. CONCLUSION 109
The use of genetic algorithm seems to be a possibility for the implementation of
the optirnization process.
4. Design of a less complex problem using the DDA approach, such as the rebalance
loop of the GyroWheelm , which would enable an implementation with fewer
integrators, possibly enabling the sequential implementation and consequently a
bet ter performance analysis, simulation and test.
Bibliography
[Bolduc 951
[Britting 711
[Chai 941
[Farrell 761
[Fejes 941
[Friedman 711
Bolduc, Carole R. M. - "Self-Alignment and Navigation Algorithms
for DREO Navigation Laboratory Heading Reference Unit", Master
Thesis. Department of Yechanic and Aerospace Engineer. Carleton
University, Ottawa, ON, 1993.
Britting, Kenneth R. - "Inertial Xavigation System .lnalysis", Wiley &
Sons Inc., New York USA? 1971, ISBX 0-471-10486X, [TL5883 8731.
Chai, O. H.; Wong, Y. S.; Poo, A. N. -"A DDA Paraboiic Interpolators
for Computer Numericd Control of Machine Tools". Mechatronics Vol.
4, No. 7, pp. 673-692, 1994, Great Britain.
Farrell, James L. - "lntegrated Aircraft Navigation", Academic Press
Inc., Xew York , NY, 1973. ISBW 0-12-249730-3, [TL588.5.F34].
Fejes, T. G. - "An Ultra-Precision Digitizer for Inertial SystemSY - Mas-
ter Thesis, Department of Mechanic and -4erospace Engineer, Carleton
University, Ottawa, ON, 1996.
Friedman, Amer - "Advanced Calculus" - Hokt Rinehart and Winston
Inc., USA, 1971, ISBN 03-083983-1, [Q.4.303.F8311.
[Grayson 001
[Hartal 971
[Hartl-1 971
[Hoeshele 94)
[Lawrence 931
[Lutz 991
Grayson, J.E. - "Python and Tkinter Programming", Manning Publi-
cations Co., USA, 2000, ISBN 1-884777-81-3.
Hartl, Derek - "Review of the Status of The TINORU 7716 Digitiser
Boards for Use in The HRU ", Carleton University, Ottawa ON, Febru-
ary 1997.
Hartl, Derek - "Heading Reference Unit (HRC) Status Review', Car-
leton University, Ottawa ON, February 1997.
Hoeschele, D. F. - cb.4nalog-to-Digitd and Digital-to-Analog Conver-
sion Techniques'', 2nd edition, John Wiley & Sons, Inc. 1994, New
York, NY: ISBN 0-471-57147-1, [TK7887.6.H6.C2].
Lawrence, A. - "Moderne Inertial Technology: Xavigation. Guidance.
and Control", Spring-Verlag, New York, NY, 1993, ISBN 0-387-9786s
2, [TLW8.5.L38].
Lutz, Mark and .Ascher, David - "Learning Python", O'Reilly & Asso-
ciates hc.. USA, 1999, ISB?i 1-56592164-9.
[Norsworthy 971 Nomorthy, S. R. Schreier, R. Ternes, G. C. - "Delta-Sigma Data Con-
verten", IEEE Press, 1997, Xew York, W.
[Park 941 Park, S. W.; Oh, J. H. - "Hardware Realization of Inverse Kinernatics
for Robot Manipulators': IEEE Transactions on Industnal Electronics
Vol. 41, No. 1, pp. 43-50, February 1994.
BIBLIOGRAPHY
[Proakis 961 Proakis, J. G.; Manolakis, D. G. - "Digital Signal Processing: Princi-
ples, Algonthms and Applications", Prentice-Hall, New Jersey 1996,
ISBX 0-13-373762-4.
[Siouris 931 Siouris, George M. - "Aerospace -4vionics Systems: a modern synthesis"
- .4cademic Press Inc.. San Diego CA. USA. 1993. ISBN 0-12-646890-7.
[TL588.5 S561.
[Sizer 681 Sizer, T. R. H. - ' The Digital Differential Analyzer", Chapman and
Hall Ltd., London, 1968.
[Staley 841 Staley. D. -4. - "AXCOX-R.829 -4naJysis of a Stable Platform Verti-
cal Control Loop", AYCON Space Technology Corporation, Ontario
Canada, September 1984.
[StaJey 981 Staley, D. A. - " Orbital Yechanics and Spacecraft Contro17', Lecture
notes Carleton University winter 98/99 course 88.5 13.
[William 981 William, H. - "Numerical Recipes in C: The Art of Scientific Comput-
ing", 2nd ed., Cambridge, England, 1998. [QA76.73.CEN865 19921.
Appendix A
Analog to Digital Converters
Analog to Digital (.4/D) converters and Digital to Analog (D/A) converters provide
the interface between the analog signal domain and the binary digital computational
domain, making it possible to interface and control sensors and equiprnent via software,
with the advantage of easy upgrade and adjustment. However, for this to be true
fast and accurate digitizers and controllers, compatible with the time constant of the
controlled system are needed.
Inertial sensors are a mature technology where sensoa can reach resolution of O.OOL
parts per million (ppm), which is beyond the resolution and dynamic range of most
digital converters. From [Fejes 941 the dynamic range necessary for the strapdown
HRU is O.OlO/hour to 400/hour (1pV to lOV), which will give us a 20-bit resolution
with an output rate of 1Hz up to 200Hz.
This appendut presents some .4/D converters, some of their advantages and disad-
vantages. The concepts wl follow the approach in reference [Hoeshele 941, but it is not
the scope of this appendk to make a complete rnalysis of the A/D converter technol*
APPENDIX A. ANALOG TO DIGITAL CONVERTERS 114
@es, instead the principal technologies available will be presented in order to justi& the
choice made to apply Digital Differential -4ndyzers to implement the navigation and
compensation dgorithms of the HRU.
Basic Concepts
The design of an A/D converter begins with an analysis of the system requirements,
where the first two requirements that must be determineci are:
dynamic range;
speed of conversion.
.in important characteristic of analog signals is the noise in the analog value that is
either inherent in the transducer, or in the circuit used to amplify the analog signai. The
value of the noise at small signal levels or the uncertainty in the analog measurement
compared to the full-scale signal value at the upper h i t of its range. determines the
dynumzc range of the usable analog signal. In other words, we can define dynamic
ronge as the ratio of the largest input that can be convened to the smallest step of
the converter. For example, a 10-bit A/D with an input of 0-1V has a quantization
IV - ov
step of = O.S?mV, therefore the dynamic range ratio is
1.OV
21
= 1024, in
0.97mV
decibels (dB) 20 log 1024 = 60dS. Table A shows values for dXerent dynumic ranges
and quantizing accuracy [Hoeshele 941 in i$ bit (%).
The speed of conue~son is a measure of conversions per second and is associated
with the t h e constant of the system.
The other requirements that d l aid the seiection of the A/'D converter are [Hoeshele 941:
Number of Binary Bits
4
a operating power characteristics;
Decimal Equivalent Vaiue 1 Quantizing Accuracy (%)
15 1 i3.3
L
16
18
20
a size:
a weight;
AJC converter range and resolution in ih bit (%) .
r
O operating environment.
65,535
262,143
Classification
10.0015
&0.00038
Nyquist Rate versus Oversampled
1 ,048!575 1 zt0.000095
According to [Fejes 941 we can classify A/D converters in hct i on of the sampling
frequency as:
APPENDUC -4. ANALOG TO DIGITAL CONVERTERS
Nyquist Rate;
Oversampled.
The fim operates near the Nyquist rate, which is twice the maximum frequency of
interest, however because of practical considerations. that is limitation in low pass
filter, hardware cutoff characteristics and limitation in digital signai processing, higher
sampling rates are often used (4 to 6 times the Nyquist rate). The convenen of this
type are:
linear pulse code modulator (PCM);
differential PCM;
O algorithm converters;
0 flash converten;
0 pipe-line converters:
O integrating converten.
The oversampled converter uses a sampling frequency much higher than the Syquist
rate, typically more than 16 t h e s higher, which allows the elhination of high per-
formance anti-alias fiiten in design, the noise is spread over the spectrum such that
baseband noise is reduced. The converters of this type are:
Delta Sigma;
Sigma.
DIGITAL CONVERTERS
Direct-Cornparison versus Integrating
[Hoeshele 941 presents another classifications of A/D where they are dividad between
direct-cornparison converters and zntegratzng converters. In the direct-cornparison con-
verters each reference to be used in the comparison is assigned a digital value. -4 simple
example is the %bit flash .4/D converter.
Other types of direct comparison A/D converters take a longer time to cornpiete a
conversion because they determine the equivalent digital value by generating a sequence
of reference analog signais by sequentially comparing the analog input against ftred
analog reference. Examples are the PCM successive approximation A/D converten
and the pipeline A/D converter.
The integrating A/D converters have a digital counter that counts the time it takes
a reference signal to integrate a charge on a capacitor to the analog input signal value'
as examples we have:
a single-dope .4/D converter;
a dual-slope AjD converter;
voltage-to-frequency (V/F) converter.
Following are some brief descriptions of these A/'D converters.
Pulse Code Modulation (PCM)
The most common PCM VI A/ converter is the successive approximation A/D converter.
This converter uses a single comparator, a successive approximation register and a
digital to analog (DIA) converter in feed back. This conversion starts with the most
significant bit (MSB) and successively tries a ONE in each bit of the D/A converter.
-4s each bit is tried, the output of the D/A converter is compared against the input
analog signal. If the DIA output is larger the ONE is removed and a ONE is tried in
the next most signXcant bit. If the analog input signal is greater than the D1.4 output
the ONE remains.
In each step of the process, sufncient tirne must be allowed for the D1.4 converter
to reach its final d u e with its accuracy constraints? then sufficient time must be al-
lowed for the comparator to recover from its previous input and to generate an output
dependent upon its new input from the D/A converter.
This .4/D converter requires n steps for encoding to an n-bit binary value. The
digital output can be taken serially, as each bit is tried, or it can be taken in parallel at
the end of of the conversion time by reading the flipflops driving the DIA converter.
Differentiai PCM
In the Dinerentiai PCM the quantizer converts the difference between the current and
the previous input sample. It is useful for inputs with a large dynamic range, especially
music.
Algorit hmic Convert ers
In an algorithmic successive approximation converter the converter calculates the er-
ror signal using a single comparator and cycle it through a loop, the analog input is
amplified and acquired by the S/H (sample and hold), then the MSB is determined by
cornparhg the S/H output against VR. If the signal is greater thanVR it is subtracted
fiom it in the feedback loop. If it is less than VR then it is passed directly around the
APPENDUC A. ANALOG TO DIGITAL CONVERTERS
feedback hop. The recirculation process continues for n cycles.
This approach for successive approximation converters elhinates the need for a pre-
cision DIA converter, but does require a precision 2X amplifier and a similar complexity
of control and readout logic circuit.
Pipeline convert ers
A pipeline converter could be created by cascading a reduced accuracy A/D converter
with a D/.4 converter and Bnding the enor signal from the original analog signal.
The error signal is converteci and the result of the next conversion will have a greater
resolution, in other words, the first stage converts a number of the most significant bits
during the fim sampled data t h e and then passes the residue to the input of the next
stage in the pipeline for processing. During the second analog-signal sampling period.
the first stage converts the MSB7s for the next analog sample while the second stage
converts the previous sample's residue to the next MSB's. The residue from this second
approximation is passed dong to the next stage, and so on until al1 bit dues have been
determined .
At the beginning of each andog-signal sarnpling period, the new sarnple of the
analog signal is entered into the first stage of the converter for its fint approximation
to digital form. The result is that each sample of analog signal taken follows the previous
sample through the string of partial converters resulting in a converter rate equd to
the sampling rate, but the full digital equivalent value is delayed in the pipeline by the
number of sample times it takes to p a s through the successive stages of the converter.
This type of converter can be used where the latency of conversion (the delay in
having the full digital equivaient value of the analog signal) is of little importance for
the applicationo and in most cases the analog sampling rate is the important criterion.
Flash Converters
These converters use one anaiog comparator, with a fixed reference voltage, for every
quantization level in the digital number from zero to full scale. The conversion rate
possible with this type of converter is extremely fast because the conversion is completed
in one step. However, since the number of comparators increases geometrically with
resolution, the amount of required circuitry is practically doubled.
The erroa in this conversion process are the comparator and the reference voltage
errors. The basic problem is typicdy not the static accuracy, but obtaining fast enough
transient response so that the true equivaient digital word is amilable within the allowed
time. .Us0 when many comparatoa detect at one t h e , significant transients can be
kicked back on the analog signal input.
Integrating A/D Converters
This type of A/D converter is applied where the analog input signal is:
a DC or slowlp v-g;
a with necessity of a high dynamic range;
a with necessity of high noise immunity.
They are simple and relatively low cost.
APPENDUC A. ANALOG TO DIGITAL CONVERTERS
Single-Slope or Ramplntegrating A/D converter
.4t the begiMing of the integration of the input signal, a switch is opened and the
output of the integrator is a constant dope voltage ramp. -4s the integator output
voltage ramps up from zero volts, the accumulator counts the cycles of the reference
clock frequency. When the voltage from the integrator equals the input analog voltage?
the comparator output changes state, the comparator inhibits the reference frequency
from entering the counter accumulator, and the final count in the accumulator is the
digital equivdent of the analog input voltage.
The advantages of this type of A/D converter are sirnplicity, high dynamic range ca-
pability, rnonotonic conversion with no missing codes and high frequency noise rejection
due to the relatively long integration of the input signal during the conversion.
The error sources are, errors in values of the resistor and capacitor in the integrator,
the reference voltage drift, dock frequency error, analog reset switching errors as well
as comparator detection point enor.
These errors make the use of this converter limited to relatively low accuracp appli-
cations.
Dual-Slope Integrating A/D Converter
To eliminate the errors inherent in the single-dope .l/D converten the dual-dope A/ D
converter is used. The basic idea is to generate a pulse width proportional to the analog
input voltage by making a time cornparison between two integrators. In this way. many
of the absolute errors in generating the integrated ramp voltage are canceled. The fint
integration is on the input analog signal. It proceeds for a fixed interval of tirne, tl .
The input to the integrating circuit is then switched to a known reference voltage. The
APPENDK A* ANALOG TO DIGITAL COLVERTEM 122
time fiom this switching until the integrator output reaches some fixed reference point
gives a mesure of the analog input voltage.
The conversion begins with the reset and a convert pulse; the counters are reset to
zero. Switch Si is closed and Sz is opened so that the time integrator begins. Since
the integrator output exceeds zero volts the comparator output changes state allowing
the counter to receive clock pulses and count until an al! ONE condition occurs. A t the
next clock signal the counter changes to dl ZERO causing the input to switch between
the reference voltage. Then the integator output voltage decreases until it crosses zero,
when the clock signal is inhibited. Reference [Hoeshele 941 proves mathematically that
this conversion process eliminates some of the worst error sources in an integrating-type
convert er.
Voltage-tctfiequency (VIF) Converter
These A/D converters generate digital output pulses at a repetition rate that is linearly
proportional to the analog input voltage. The pulses are counted for a fked penod of
tirne? the conversion tirne period The digital value is the average value since the pulse
rate will increase or decrease as the analog signal VIA increases or decreases during each
conversion time period.
For a petfect V/F converter, the theoretical frequency of the output pulse trains fo
is :
where:
APPENDIX A. ANALOG TO DIGITAL CONVERTERS
VIA is the analog input voltage;
a VFS is the maximum full-scale input voltage; and
a f FS is the maximum frequency for full-male input voltage.
Charge Reset V/F Converter
q;i;& L-r,
control dL
wunter i 1 ,
- , , , J
Figure Al: Charge reset V/F converter [Hoeshele 941.
The operation consists of the capacitor Cl charging untii the voltage on -4 is equal to
VREF the reference voltage. The comparator triggers the one-shot multivibrator which
closes the switch Si discharging the capacitor Ci . This sequence sets the period for
one cycle of voltage-to-frequency conversion.
The sources of error are the resistor Ri, the capacitor Ci, the reference voltage VREF
and the comparator detection error.
Charge Balancing V/F Converter
This type of V/F converter provides a more accurate V/F converter because it does
not depend on the capacitor Cl value.
The integrator is an operational amplifier with its negative input held at virtual
gound by the negative feedback from Ci. The input current is then directly pro-
portional to FA, where IIrl = !&- As the integrating capacitor Cl discharge, the
integrator output voltage on A ramps linearly in a negative direction. When it reaches
the zero-volt reference on the comparator, the comparator detects and triggea the
one-shot multivibrator. During the one-shot's on time, t, , the reset current, IR, puils
charge fiorn capacitor Cl as the output voltage on A linearly ramps in the positive
voltage direction. At the end of the one-shot on time the one-shot's turns off, thereby
opening Si, the end of the rest period. The value of IR is larger than IIA since during
reset it must be equal to IIA plus the current necessary to reset the capacitor for the
next discharge integration.
This circuit, as the charge reset V/ F converter, is a free-nuining circuit, which means
that once it is powered the circuit will st art itself and generate an output frequency
proportional to the input voltage.
Summarizing, it is an integration of a constant current source, with an analog com-
parator to detect when the ramp has reached a reference value.
Volts
A
' -\
Volts
Multivibrator ".\
I L
Reset 1 1
O
1 I - T ime
l ntegrate
Figure -4.2: Charge balancing V/F converter [Hoeshele 941.
The value of fo using the circuit elements shown in figure A.2 is [Hoeshele 941:
From this expression we have the scale factor for converting VIA to fo and shows us
the principal sources of errors.
Effect of Cl Although the value of Cl is not the determinate of fo, 9 must be large
enough to absorb the charge during the reset time without saturating the integrator
amplifier and not too large so that a reasonable dope occurs at the comparator input
for timely comparator detection. It is assumed that the value of 4 is constant for the
value of fO show above. If dielectric absorption is significant the charge absorbed
due to dielectric absorption is released slowly nom CL and may not be dischargeci
completely during the discharge time. This causes an effective variation in CI and
results in a heari t y error, since KA values would have a faster than predicted discharge
time and therefore a higher fo than predicted for a linear conversion. Polystprene or
?iPO ceramic capacitoa are preferred for temperature coefficient and low dielectric
absorption; polycarbonate and mica may be acceptable [Hoeshele 941.
Oversampled Converters
Delta Modulation
The principle of the delta modulator is to quantize the change in the signal from sample
to sample rather than quantize the absolute value of the signal at each sample. The
integrator in the feedback acts as a predictor, trying to minimize the error at each
APPENDK A. ANALOG TO DIGITAL CONVERTERS
sample. the error term is used to make the next prediction.
Delta-Sigma (AC) Modulation
AC oversampled .4/D converters have in recent yean become more prevdent for high-
accuracy, 12-bit to beyond 20-bit, A/D conversion of DC through moderately high
(hundreds of kilohertz) AC signals . Their greatest advantage is that they trade greatly
reduced analog circuit accuracy requirements for increased digital circuit complexity
[Hoeshele 941. The result is high precision at low cost.
The basic oversampled delta-sigma A/D converter is an integrating type A/D con-
verter. The single-bit feedback D/A converter output is subtracted from the analog
input signal, VIA, in the summing amplifier. The resultant error signal from the sum-
ming amplifier output is low-pass filtered by the integrator and the integrated error
signal polarity is detected by the single comparator. This comparator is a one-bit -4iD
converter. The output of the comparator drives the 1 bit D/A to a 1 or O; a 1 if dur-
ing the previous sample time the integator output was detected by the comparator
as being below OV; a O if the difference detected during the previous sample time was
above the 0V reference of the comparator. The 1-bit DIA converter, as in successive
approximation A/D converter, provides the negative feedback. Then for small values
of VIA the htegrator would take many clock pulses to cross OV, after a single 1 was
generated, during which time the comparator is sending 0's to the digital filter. If VIA
were at full scale, the integrator would cross zero every clock time and the comparator
output would be a string of altemate 1's and 0's. The digital filter's hc t i on is to
determine a digital number at its output that is proportional to the number of 1's in
the previous bit Stream fiom the comparator. Vanous types of digital tilters are used to
perform this computational function, which is the most complex function in this type
of A/D converter.
Appendix B
Description of the Simulation Software
Files
The DDA integrator class TIntegrator and two collecter classes the TMultiplyDDA,
which implements the DDA multiplier, and TSquareDD-4, the square DDA implemen-
tation axe implemented in integr. cpp, as presented in section 3.5.
class TIntegator
protected:
long Y;
long R;
// register Y
// register R
long l i m i t ; // maximum value that Y and R can assume
int N ; // size of Y and R i n bits
char flgY; / / flag for no-linear ansver of
// integator if 1
APPENDIX B. DESCRIPTION OF THE SIMULATION SOFTWARE FILES 130
char flag; // flag of overflov - 1 when it occurs
public :
char dz; // output of the integator
TIntegrat or (long Y0 , long RO ,
int NO, char flgYO) ;
'TIntegator O ;
void setY (long Y01 ;
void setR(1ong ROI;
long getY 0 ;
long getR() ;
char getflago ;
char inteqate (char &ci, char dyi) ;
1
class TSquareDDA

privat e :
TIntegator *integr;
int N;
// si re in bits of the registers
char f lag; // overflow flag
public :
TSquareDDA (long Y0 , int N) ;
TSquareDDA(1ong Y0,int N,char flagYO);
'TSquareDDA () ;
long getY O ;
char getflagO ;
char squarekhar dx) ;
>
class TMultiplyDDA
<
private :
APPENDIX B. DESCRIPTION OF THE SIMUL.4TION SOFTWARE FILES 131
TIntegator *integr1 ;
TIntegrator *integr2 ;
int N ;
// size in b i t s of the registers
long *Y; // Y registers
char f lag ; // overflou flag
public :
RIultiplyDDA (l ong YI. l ong Y2, int NI ;
R(u1tiplyDDA (long Y 1, long
long *getY () ;
char getf lag
char multiply (char du, char dv) ;
dda - alg.cpp
TDDA-.Ug is the base cl as for ail the algonthm classes, it has the basic attributes
cornmon to dl the navigation classes and is implemented in this file.
class TDDA-Alg

protected:
TIategrator **integr ;
TMultiplyDDA **mi t pl
TSquareDDA **square ;
// integrators
; // multiply integrators
// square integrators
i nt N ;
// sire of the integrators registers
i nt Nint;
// number of integrators
i nt Nmlt;
// nuber of multiply integrators
int Nsqr ;
// number of square integrators
long *YiO; // i ni t i al value of the integrators
long *YmO; // i ni t i a l value of the multipliers
long *YsO; // i ni t i ai value of the square
char flag; // flag the error if different of zero
void I ni t i ai i re 0 ;
void I ni t i al i ze (char f lagYO) ;
public :
int NmltO, i nt NsqrO, i nt NO) ;
'TDDA-Alg O ;
char getflag0 ;
long *getintY O ;
long *getmltY O ;
long *getsqrY O ;
int ConstraintO; // veri fy the constraints of each
comp. cpp
This is the file that implements the compensators for the angular rates and accelerations
sensed through the gyroscopes and accelerometers.
TAccelComp
This class implements the DDA compensator for the accelerometers data as presented
in section 4.2.1 figure 4.2.
class TAccelComp :public TDDA-Alg
-4PPENDIX B. DESCRIPTION OF THE SIMULATION SOFTWARE FILES 133
double armax; // maximum acceleration raw value
double Mk; // supremum of AK
double Kar;
// accel i ntegator scale factor
double Ky; // Ky f or the integrators
char *adt; // output of the integrators
char *da; // output of the i nt egat ors
public :
TAccelComp (double *AB0 , double * M O ,
double MkO , double A0C31 C3 ] ,
double amaxO, double tao,
int NI ;
'TAccelComp~) ;
long *getA 0 ;
char *getadt 0 ;
char *getdaO ;
double getMk() ;
nt ConstraintO; // veri fy the constraint
char *integrate (char * du, char dt ) ;
1
TW - Comp
This class implements the compensation algorithm for the gyros data. It follows the
algorithm and diagram of figure 4.3 in section 4.2.2.
cl ass TU-Comp:public TDDA-Alg
<
double max; // maxi mum angular vel oci ty raw value
double Mnr; // supremum of Kuij terms
APPENDM B. DESCRIPTION OF THE SIMULATION SOFTWARE FILES 134
double Mka; // maximum value of Kaij terms
double Kur; // scal e f act or f or integrator
double Kyv; // Ky f or the integrators
double Kya; // Ky f or t he integrators
double Ku; // scal e f act or f or integator
char *wdt; / / output of t he integrators
char *du; // output of the integrators
char f l gconst rai nt ; // Constraint f l ag 1=Ok; Osfail
publ i c :
TW-Comp (double GKO [4] , double GO [4] 141 ,
double Kai j Cg] , double vrmax0,
double v0 [3] , double MkwO,
double MkaO, int N) ;
'TW-Comp (1 ;
char *get dvo ;
long *getW O ;
double getMkv0 ;
i nt Constraint 0 ; // veri f y the constraint
char *integate(char *da, char *dur, char dt ) ;
>
This file hplements the classes that integrate the DCM elements and the quaternions.
TQ, TD and Tqi have each an instance of the orthogonalization class.
TQii, TQ and TQ
The DCM Q is the direction cosine m a t e that transfom a vector in the system frame
of reference to the computational frame of reference as a function of the quaternions. It
has two major classes, the one which implements the diagonal elernents called TQii, as
in figure 4.5 (a), and TQij which implements the non-diagonal elements, as presented
in figures 4.3 (b), (c) and (d). TQ is the coilector of TQii and TQij.
class TQii: public TDDA-Alg

double qC4J ; // i ni t i al value of the quaternions
double Kq; // quaternions scale factor
int NQ; // sire of the DCM Q i ntegators
char *dQii; // output of the integators
vo i d I ni t ialize (char f lagYO 1 ;
// override the TDDA-Alg one
// allov the different size of
// integators
public :
TQii(doub1e *qO, double M2,
double KqO , int Nq, int NQO) ;
-TQiiO ;
char *getdQii O ;
int ConstraintO; // verify the constraints
char *integrate(char *dq) ;
3
APf ENDIX B. DESCRIPTION OF THE SIMUL14TION SOFTW-4RE FILES 136
cl ass TQij: public TDDA-Al g

double q [43 ; // i n i t i a l value of t he quaternions
double Kq; // quaternions scale f act or
i nt NQ ;
char *dQi j ; // output of the i nt e ga t or s
void I ni t i a l i z e (char f lagYO) ;
// override the TDDA-Alg one t o
// a l o v the different si ze of
// integators
public :
TQi j (double *qO , double M2,
double KqO , i n t Nq , i nt NQO) ;
'TQijO ;
char *getdQi j 0 ;
i nt Const rai at (); // ver i f y al1 t he const r ai nt s together
char *integrata (char *dq) ;
>
class TQ: public TDDA-Alg
<
TQi i *Qi i ; // diagonal elements
TQij *Qij; // non-diagonal elements
double KQ; // Q terms scal e f act or
int NQ ;
char **dQ; // increments
double **Q; // Q elements
TDCn-Onho *Q-Ortho ; // Q terms orthogonalizat ion
public :
TQ (double *g , double Q0
131 ,
APPENDUC B. DESCRIPTION OF THE SIMULATION SOFTWARE FILES 137
double M2, double KqO,
int Nq, i nt NQ) ;
"TQO ;
char **getdQ() ;
double **getQ O ;
char getf lagO ;
int Constraint () ; // veri fy al1 the constraints together
char **integrata (char *dq, char dt) ;
TD is the implementation of the integratoa of the elements of D: the DCM that
transforms coordinates fiom the earth-centered earth-Lxed frame of reference to the
computationd frame of reference, as presented in figure 4.6 section 4.3.3.
class TD: public TDDA-Alg
I
double rho[2] ; // value of r h o
double **D; // values of D elements
TDCH-Ortho *D-Ortho; // Q terms orthogonalization
double KVc; // T m oelocity scale factor
double KD; // D terms scale factor
double MRo; // supremum of Ro
double MD; // supremum of D elements
double Ro; // constant Ro earth radius
char **dD; // output of the multipliers
char drho l21; // output of the integrators
public :
TD (double trho0, double DO C31 C33 ,
double MRoO, double RoO,
double KVcO, int N) ;
char getf l a g 0 ;
char **getdD O ;
double **getDO ;
int Constraint 0 ; //
char **integrata (char
>
verify al1 the constraints together
*dVc,char dt ) ;
This class implements the quaternions differential equation as presented in section 4.3.6
figure 4.10.
class Tqi: public TDDA- Al g
<
double m[3] ; // i ni t i al vaues of mu
double q[4] ; // i ni t i al values of q
double b u ;
double Kq;
double K2q; // scale factor of the 1/2
// constant multiplier
char *dz; // output of i ntegators
char *dq; // quaternions increment s
TOrtho *q-Ortho ; // quaternions orthogonalization
public :
Tqi (double *mu0 , double *q0, double *dqdtO ,
doubla KmuO, double KqO, double M2q0,
r q i o ;
int N) ;
char getflagO ;
char *getdqO ;
int ConstraintO; // verify al1 the constraints together
char *i ntegate (char *cimu, char dt) ;
>
nav2.cpp
This file implements the classes that are responsible for the angular velocities, acceler-
ation and velocity components.
TDamped - rates
This class implements the damped spatial rate presented in [Bolduc
4.3.1 figure 4.4.
cl ass TDamped~ate:public TDDA-Alg
1
double nu[2] ; // initial vaiues of nu
double KVc;
double Mrho;
double Krho;
double MTau; // supremum
double MR;
double Mact;
double Tau;
double R;
951 as in section
double a; // constant of the z damping component
char *dz ; // integrators output
char *&ho; // output of the i ntegrators
publ i c :
Ibamped-rat e (
double Tau, double RO,
double aO, double *nuO,
double *rhoO, double KrhoO,
double MrhoO, double KVcO,
double MTauO, double MRO,
double MactO, int N);
TDamped-rate 0 ;
char *getrho O ;
double *getV 0 ;
int Const rai nt o ;
char *i ntegrate (char *dV, char d2Vy, char dt) ;
>
TMuc
This class implements the true angu1a.r velocity relative to the computational kame of
reference p, as presented in section 4.3.4 figure 1.7.
class TMuc:public TDDA-Alg
.E
double Mhega; // supremum of omega
double Ku;
double KQ;
double Kmu;
double Omega;
char *cimu; // output of the i ntegrator
public :
Tnuc (double 90 133 [3] , double *wO ,
double Kw0,double KQO, double MOmega0,
double OmegaO , i nt N) ;
-mut O ;
char *getdmuO ;
int Constraint 0 ;
char *i nt e vat e (char **dQ , char *du,
char *&ho, char **CD) ;
This class implements the tme acceleration relative t o the computationd frame of
reference as presented in section 4.3.5 figure 4.8.
class TAc:public TDDA-A1g
<
double Q C31 C31; // i ni t i al vaiue of the Q matrix elements
double aL31; // initial value of the accel erat i on components
double Ka;
double KQ;
char *dAc; // output of the i ntegator
publ i c :
TAC (double QO [3] [3] , double *a0 ,
double KaO ,double KQO, int N) ;
T A C O ;
char *getdAc O ;
i nt Constraint 0 ;
char *i nt e vat e (char **dQ , char *da) ;
TVc
This class impiements the true velocity relative to the computational frame of refer-
ence as presented in section 4.3.7 figure 4.11. The d2Vc is the derivative of Y,, it is
implemented since it is needed as input to calculate pz.
c l as s TVc:public TDDA- Al g
I
double Krho;
double KD;
double KAc;
double KVc;
double Kgz;
double B [3] ;
double MB;
double WC;
double Omega;
double WZOmega; // supremum of Omega (earth rate)
char +dVc; // output of the integrator
char *d2Vc; // output for the damped rates
public :
TVc (double D C31 C31, double *rho ,
double *VcO, double *AcO,
double KVCO. double KrhoO, double KDOD
double KAcO, double K t ,
double M2hega0, double OmegaO,
double *BO, double gzO, int Bi) ;
-mco ;
char *getd2Vc ( ) ;
char *getdVc () ;
i nt Constraint 0 ;
char *intevate (char **dD , char *&ho,
char *dVc , char *dAc, char dt) ;
>
navigat .cpp
This file contains the navigation class T?iavigation which uses al1 the others classes
defined in navi.cpp, nav2.cpp and comp.cpp. It also defines the TNavInitdt, the
configuration data structure used by TSimull ro configure the navigation algorithm.
s t r uct TNavInitdt {
double AB[3] ; // accelerometers bi as
double ; / / accelerometers scale f act or s
double AC33 C31 ; / / A DCM matrix of misalignment
double OB [4] ; / / angular r at e bi as
double GKC41; // gyros scale f act or s
double MU[4]; // t he mass unbalance parameters
double Qd[4] ; // t he quadrature parameters
double OC41 [4] ; // G DCM matrix of misalignment
double KaijE91; // compensator parameter
double al31 ; // initial value of t he accel erat i on components
double w[4] ; // initial value of the angular r at e components
double Ac131 ; // initial value of t he true accel erat i on components
double VcC31; // i ni t i al value of t he t r ue velocity components
double q[4]; // i n i t i a l value of t he quaternions
double rhoC31; // i n i t i a l value of rho
double mu C3l; // i n i t i a l values of mu t r ue angular r at e
double nuC31; // i n i t i a l values of nu dumped vel oci t y
double dqdt [41;
double QC31 C31; // i n i t i a l value of DCM Q
double DL31 [3]; // i n i t i a l value of DCM D
double Tau;
double act ;
double Ro;
double R;
double mega;
double B [3] ;
double gm;
i n t N a r ;
iat Nur ;
i n t Nrho ;
i n t NQ ;
i n t ND;
i n t Nmu;
i n t NAc;
int Nq;
iat NVc ;
double armax;
double Mis;
double urmax;
double Mkw ;
double Mka;
double Mrho;
double Mmu;
double M'Tau;
// damping f ilter constant
// damping constant of z axi s
// constant R euth radius
/ / constant R of damping
// ear t h r a t e
// damping constants
// g a v i t y accel erat i on
// s i ze of Accel-Comp r egi st er s i n b i t s
// s i ze of U-Comp r egi s t er s i n b i t s
// size of Damped-Rate i n bi t s
// size of DCM Q terms i n bi t s
// size of DCM D terms i n bi t s
// s i ze of True angular vel oci t y i n bi t s
// s i ze of t r ue accel erat i on i n b i t s
// s i ze of quaternions i n bi t s
// s i ze of t r ue velocity i n b i t s
// maximum accel erat i on rau value
// supremum of AK (Accel-Comp)
// maximum angular vel oci t y raw value
// supremum of Ku terms i n W-Comp
// supremum of Ka terms i n U-Comp
// supremum of damped rate
// supremum of true angular vel oci t y
// supremum of Tau (damping parameter)
APPENDIX B. DESCRIPTION OF THE SIMULATION SOFWARE FILES
145
double Mact;
double MR;
double MRo;
double Mmega;
double M20mega ;
double MAC;
double MVc;
double M2;
double M2q;
double K t ;
double Kar;
double Kur;
double Ka;
double Ku;
double KQ;
double KD;
double Krho;
double Kmu;
double Mc;
double Kq;
double KVc;
3
// supremum for act (damping paran
// supremum of R (damping constant)
// supremum of Ro (earth radius)
// supremum of omega
// supremum of 2*omega
// supremum of true acceleration
// supremum of true velocity
// supremum of the 2 multiplier i n DCM Q
// supremum of the 1/2 multiplier i n quaternions
// time integrators scale factor
// acceleration rau scale factor
// angular velocity raw scale factor
// acceleration scale factor
// angular velocity scale factor
// DCM Q terms scale factor
// DCM D terms scale factor
// damped rate scale factor
// tnxe angular velocity scale factor
// true acceleration scale factor
// quaternions scale factor
// t ni e velocity scale fact or
class TNavigation
TNavInitdt *NavInitdt ;
TAccelComp *AccelComp; // accelerometer compensator
T-Comp *W,Comp; // angular velocity compensator
TQ *Q; // i nt egat es the DCM Q components
TD *D; // i nt epat es t he DCH D components
Tqi *qi; // i nt egat es the quaternions
TDamped-rate *Danped-rate ; // i nt egat es the damped rates rho
TMuc *Muc;
// i nt e ga t e s the t r ue angular vel oci t y
TAC *Ac;
// i nt egrat es t he true accel erat i on
WC *Vc;
// i nt e ga t e s the t r ue velocity
char *adt; // compensated accel er at i on
char *da; // compensated accel er at i on
char *du; // compensated angul ar velocity
char *&ho; // dumped angular rate
char *dmu; // true angular r a t e increment
char * Mc; // true accel er at i on increment
char *dq; // quaternions increments
char *d2Vc; // true vel oci t y der i vat i ve increments
char *dVc; // tnie vel oci t y increments
char **dQ; // DCM Q components increments
char **a; // DCM D cornponents increments
double **DCW 121 ; // pointer t o t he DCM Q and D
double *at t i t ude; / / at t i t ude parameters
double *position;// position parameters
l ong count;
// i t er at i ons count f o r python pipe
// python pipe parameters
long i t er at i ons ; // i t er at i ons l i m i t f o r python pipe output
iat f 1gPr ;
// print t o stdin flag f o r python pi pe
long i t count ; // i t er at i ons count er
public :
TNavigation (TNavInitdt *NavInit ,
long mult i 0, i nt f l gPr ,
long i t erat i ons0) ;
'TNavigation O ;
double *getAtt itude (1 ;
double *getPositionO ;
i nt ConetraintO; // verify al 1 the constrainta together
char integrate (char *dar , char *dur, char dt) ;
void SetInitialCondition(TNav1nitdt *NavInit);
>
This file is the implementation of the orthogonalization algorithm for the DCM terms
and the quaternions, as presented in section 4.4.
The class TOrtho is the implernentation of the quaternion's orthogonalization as in
figure 4.12, and the class TDCM-Ortho is the implementation of the DCM onhogo-
naiization aigorithm as in figure 4.13.
class Mrtho:public TDDA-Alg
<
i nt nterms; // number of t erm 4 for
// quaternions 3 for DCM
double Mv; // supremum of the terms
double Me; // supremum of epsi l on
double epsilon;
char *dv,; // inteqators output
public :
TOrtho (double *v0, i nt nterms0, i nt N) ;
'Tortho O ;
char *getdO ;
char *intepate (char *dv0) ;
.4PPENDUC B. DESCRIPTION OF THE SIMULATION SOFTWARE FILES 148
class TDCM-0rtho:public TDDA-Alg

double MD; // DCM D matrix supremum
double Me; // supremum of epsilon
double epsilon;
char **dD,; // output of the i nt egat or
char *dMlt; // output of the multipliers
public :
TDCM-Ortho (double **DO, i nt N) ;
"TDCM-Ortho 0 ;
char **getdD 0 ;
char **intevate (char **db) ;
3
simull .cpp
This file is responsible to read the configuration file h m . cf g, fil1 the data structure
TNavlnitdt with al1 the parameters for the integrators, and to create the instance of the
TNavigation class that will be responsible for running the navigation and compensation
class TSimull

TNavInitdt *NavInit;
char f ilecfg[WATIIl; // Config f i l e nana
char f ileinacc [UPAm ; // Input data f i l e name
APPENDIX B. DESCRIPTION OF THE SIMULATION SOFTW-4RE FILES 149
char f i l e i ngyr ~WATH] ; // Input data file name
char f i l eout [MAXPATKI ; // Output f i l e name
char f i l eQout [MAXPATH] ; // Output f i l e name
char f ileDout [IWtPATH] ; // Output f i l e name
FILE *f cf g;
FI LE *f inacc ;
FILE *fingyr;
FILE *f out ;
FILE *fQout ;
FILE *fDout ;
i nt *dur ;
double *accr;
double *Ur;
double Mkread,Mkuread;
double pos CS] ;
double att [3] ;
double Clmead31 C31;
double Dmean C31;
long II;
char flag;
i nt f l gPr ;
// pointer t o c f g input f i l e
// pointer t o data input f i l e
// pointer t o data input file
// pointer t o dat a output f i l e
// pointer t o data output f i l e
// pointer t o data output f i l e
// raw increments
// raw increments
// raw accelerometer dat a
// raw gyr o data
// values from t he cfg f i l e
// l at i t ude, l ongi t ude, wander
// r ol l , pi t ch, yaw
// mean values of Q
// mean value of D
// counter of elements t o mean cal cul at i on
// python flag O->Python output
void Initialize(char *inacc, char *ingyr.
char *Qout, char *Dout,
char *out, int flgPrO,
long i t er at i onso) ;
void InputCfgO ;
void ReadCf g o ;
void UritaCfgO ;
void Fi l l NavIni t 0 ;
i nt ReadDatao ;
voi d WriteDataO ;
public :
TNavigation *Navigation;// the navigation algorithm
TSimull(char *inacc,char *ingyr,
char *Qout , char *Dout , char *out,
i nt f lgPrO . long iterations0) ;
TSimull(char *cfg.char *inacc,
char *ingyr. char *Qout,
char *Dout,char *out, int flgPrO,
long iterations0) ;
TSimull(char *cfg. double *accru,
double *urO, i nt flgPrO,
long iterationso) ;
'TSirnullo ;
voi d set Initiais (double *w0, double *a01 ;
The classes TIntAcc and TIntGyr uses TSpline to implement the cubic spline interpo-
lation of the accelerometer and gyroscope data, respectively, frorn the input files. The
interpolation algorithm implemented in TSpline is based on the algorithm presented in
[Wiliiam 981.
APPENDIX B. DESCRIPTION OF THE SIMULATION SOFTW-ARE FILES 151
cl ass TIntAcc
protected:
FILE
FILE *fout;
int res ;
i nt mult ;
long N;
long nO ,nf ;
long si ze;
// number of poi nts
// si ze of the buffer
long newN;
double *t,**acc;
TSpline **spline ;
i nt ReadDatao ;
public :
TIntAcc (char *FileIn, char *Fileout,
long NO. long Nf , int Mult) ;
TIntAcc (char *FileIn, long NO, long Nf) ;
-TIntAcc () ;
double Interpolate (i nt 1, double x) ;
void getAcc(long n, double *resacc) ;
double get'Nlong n) ;
int RunO;
>
cl ass TIntGyr

protected:
FILE *fin;
FILE *fout;
i nt res;
i nt mult ;
long N; // number of points
long n0,nf;
long aire ; // size of the buffer
long neuN;
double *t , **gyr ;
TSpline **spline;
i nt ReadData (1 ;
public :
TIntGyr (char *FileIn, char *Fileout,
long NO, long Nf , int Mult) ;
TIntGyr(char *FileIn, long NO, long Nf) ;
'TInt G y r 0 ;
double Interpolate (int 1, double x) ;
void getGyr(1ong n, double *resgyr) ;
double getT(1ong n) ;
i nt R u n o ;
1
in this file is implernented the class TSpline which is responsible for the cubic B-spline
interpolation used by TXntAcc and TIntGyr. The interpolation algorithm is based on
the algorithm presented in [William 981.
class TSpline
protected :
double *x;
double *y;
double *coef ; // coef icients
long N; // number of points
double deriv0; // boundary
double derivN; // conditions
void Sort0 ;
void Spline 0 ;
public :
TSpline(doub1e *x, double *y, long N D
double ypn, double ypl) ;
TSpline (double *x, double *y, long N) ;
'TSpline (1 ;
double splint (double y) ;
double *getCoef s O ;
>
navsimul. cpp
This is one of the main files of the navigation/simulation implementation. It calls the
spline routines through instances of TIntAcc and TIntGyr, finds the coefficients of the
Bsplines, and then, for each new point interpolated, finds the incrernents and nuis the
navigation algorithm through the instance of TSimul.
class TNavSimul

// configuration inf omat ion
FILE +f inout ; // pointer t o i nout xf g f i l e
FILE *fhru; // poi nt er t o hni.cfg f i l e
char i n Di r [ WATH] ; // input directory
char outDir [MAXPITH] ; // output directory
char i d c c CIYAXPATH] ; // input acceleration f i l e
FILE *finAcc; // poi nt er t o input acceleration f i l e
char inGyr[MAXPATJ ; // input gyros data f i l e
FILE *f inGyr; // poi nt er t o input gyros dat a f i l e
char outAtt [MAXPATH] ;// output at t i t ude f i l e
FILE *f outAtt ; // poi nt er t o output at t i t ude f i l e
char outPos MXPATH] ; // output position file
FILE *f outPos ; // poi nt er t o output posi t i on f i l e
char outMean [MAXPATH] ; // output mean f i l e
FILE *f outMean; // poi nt er t o output mean f i l e
i nt f lgPr ; // output fl ag 1->fi l e O->stdout
TInt Gyr * intgyr ;
TSimull *simul;
char * d a ;
char *dur;
long N0,Nf;
long N;
long f r eq;
double a t t [3] ;
double pos [3] ;
double meanatt t31;
double meanpos C31;
char flag;
void WriteDatao ;
// 0-spline i nt erpol at es acceleration
// 8-spline i nt erpol at es f or gyros dat a
// raw increments
// raw increments
// i n i t i a l and f i na points of t he sample data
// mean values counter
// sampling frequency
// at t i t ude
// posi t i on
// mean at t i t ude
// mean poeition
// error fl ag
// mite the results t o t he output f i l a s
public :
TNavSimuUint f l g , long iterat ions01 ;
-TNavS imul() ;
mainnav. cpp
This is the file where the main hinction is implemented. It creates the instance of the
TrJavSimul and verifies the command line input, to generate the output to be piped
through python to be plotted by the stripchart . py script.
i nt main(int argv, char *argc 0 )
i
int f l g = 1; // not python interface
long iterations = 10;
// read the possible flg as input command
// (O->output to stdout (Python pipe) )
if (argv > 2)
<
f l g = atoi(argc(l1) ;
iterations = atoi ( argc 123 ) ;
>
else if (argv > 1)
f lg = atoi (argc Cl] 1 ;
TNavSimul *NavSimul = nev TNavSimul (f lg , iterat ions) ;
APPENDIX B. DESCRIPTION OF THE SIMULATION SOFTWARE FILES
156
NavSimul->ru O ;
delete NavSimul;
return O;
3
This is the generator of data to the navsimul program. It was first used to generate
piped data through python for the fint tests of st r i pchar t .py, but it was also used
to generate the data files used for the static vehicle test of the navsimul progam with
the simulation files from Carole Bolduc's work [Bolduc 951.
The data output format is the same as the DREO tiles. It generates data points
for a static vehicle in a defined position and attitude, with the corresponding bias and
drift terms of the accelerometen and gyros. To define these parameters it uses the
configuration file datagen . cf g.
class TDataGen
<
double vander ;
double latitude ;
double longitude ;
double roll;
double pitch;
double yaw;
double g;
double omega;
APPENDIX B. DESCRIPTION OF THE SIMULATION SOFTW.4R.E FILES 137
double GK;
double *w;
double *GB;
double *Qd;
double *MU;
double *a;
double *AB;
double **Q;
double **D;
long count;
FILE *f accdata;
FILE tfgyrdata;
void InitMatrices 0 ;
void GenerateDt 0 ;
public :
TDataGen (double wan , double l a t ,
double lon, double ro,
double pi t , double ya, double go,
double omega0, double *MO,
double GKO, double *CBO,
double *MUO, double *QdO,
FILE *f accdt , FILE *f gyrdt) ;
'TDataCenO ;
void NeuOrientat ion (double uan,
double lat , double lon,
double r o, double p i t , double ya) ;
void OutputDt 0 ;
1
Appendix C
Configuration Files
APPENDUC: C. CONFIGURATION FILES
G
urO
aro
vco
Tau
act
Ro
R
Omega
B
l at
long
wander
r o l l
pitch
Yaw
N a r
Nwr
NQ
arma%
Mk
m a x
Mku
APf ENDIX C. CONFIGURATION FILES
MTau
Mact
MR
MOmega
M2
M2q
Kt
Input D ir=
OutputD izz
InputAcc=
InputGyF
ResultAtt=
ResultPos=
ResMean=
NO=
Nf =
wander
lat
long
/
* /
bru-acc . dat
hni-gyr . dat
res-att -2nd
ras-pos.2nd
mean. 2nd
1
1461
APPENDUC C. CONFIGURATION FILES
roll
pi t ch
Yaw
Omega
gm
AB
CK
GB
MU
Qd
AccData
GyrData
Npts
acc-data . dat
-,data. dat
200
stripchart . conf
O . autorange
1. iterations
2. shiftby
3. numgraphs
5.ylarishide
6 . papht t l e s l
7. grapht i t l esl
8. gapht it les3
10000 * O
1000
1000
9 4.ynumpts C4,3,3,3,6,3,3,3,63
Y==
Quaternions
Tnie_Anguiar,Rates
Dmped-Rat es
APPENDIX C. CONFIGURATION FILES
9. gr apht i t l es4 D i i
10. gr apht i t l ess D i j
11. gapht i t l es 6 Velocity
12 . ga pht i t l e s 7 Acceleration
13 . gr apht i t l es8 Qii
14 . ga pht i t l e s 9 Qi j
15. l i n e t i t l e s l ["qlU , fi1q2h, laq3I1 , "q4I1 ]
16. l i net i t l e s 2 ["muxM, "muyl' , "muzN J
17. l i net it les3 C1khox" , "rhoyl' , larhozfl]
i 8. l i net i t l es 4 C M1 l " , "22", "33"J
N. l i ne t i t l e s 5 ["12", It13lt, I121lt, "23" , "3118 , tt32H]
20. l i net i t l es6 [I8Vxb1 , Vyl1 , VZ~ ~ ]
21. l i net i t l e s 7
["Axl' , IaAy1l , tlAzll]
22. 1i net i t l es8 ["1lU, "22", "33"]
, 23" ,tI31" "
23. l i net i t l e s 9 ["1211, "131' , "21" II , 32"]
24 . ps f i l es l q*Ps
25. psf i l e s 2 mu. ps
26. psfi l es3 rho .ps
27. psf iles4 pos .ps
28. psf iles5 att . ps
29. psf i l e s 6 vc . ps
30. psf i l es7 ac . ps
3l . psf i l es8 Qii .ps
32. psf iles9 Qij .ps
APPENDIX C. CONFIGURATION FILES
s t r i ~ g r a ~ h . conf (640~480)
O. tickfont -Clean-Bold-R-Normal-*-7-140-4-
1. vidth 640
2. height 430
3. graphvidth 220
4. graphheight 140
opt ionDB (640x480)

MQ 52392

Hochgeladen von

Dokumentinformationen

Originalbeschreibung:

Originaltitel

Copyright

Verfügbare Formate

Dieses Dokument teilen

Dokument teilen oder einbetten

Freigabeoptionen

Stufen Sie dieses Dokument als nützlich ein?

Sind diese Inhalte unangemessen?

Copyright:

Verfügbare Formate

MQ 52392

Hochgeladen von

Copyright:

Verfügbare Formate

A New Digital Differential Analyzer Approach for

Inert ial Technologies

Das könnte Ihnen auch gefallen