IMU-BASED INERTIAL NAVIGATION – ALGORITHM DESIGN USING
ADAPTIVE KALMAN FILTERING AND DIRECTION COSINE MATRICES
by
Carlos BARRIOS
A Graduate Paper Submitted
in
Partial Fulfillment
of the
Requirements for the Degree of
MASTER OF SCIENCE
in
Electrical Engineering
Approved by:
PROF. ___________________________________________________________
PROF. ___________________________________________________________
DEPARTMENT OF ELECTRICAL ENGINEERING
COLLEGE OF ENGINEERING
ROCHESTER INSTITUTE OF TECHNOLOGY
ROCHESTER, NEW YORK
NOVEMBER, 2006
i
TABLE OF CONTENTS
List Of Figures ................................................................................................................... iv
List Of Symbols and Nomenclature ................................................................................... vi
1
Introduction .................................................................................................................. 1
1.1
2
The METEOR Project at RIT ............................................................................. 2
Review Of Literature .................................................................................................... 4
2.1
Inertial Navigation Systems ................................................................................ 4
2.2
GPS Network ...................................................................................................... 6
2.3
Inertial Measurement Units................................................................................. 8
2.3.1
Gimbaled ......................................................................................................... 8
2.3.2
Strapdown ..................................................................................................... 10
2.3.3
MEMS Inertial Sensors ................................................................................. 10
2.4
Direction Cosine Matrices ................................................................................ 12
2.5
Quaternions ....................................................................................................... 15
2.5.1
Quaternion rotation ....................................................................................... 16
2.5.2
Conversions from quaternions to DCMs ...................................................... 17
2.5.3
Choosing between quaternions and DCMs ................................................... 19
2.6
Sources of Error ................................................................................................ 19
2.6.1
Bias and Drift ................................................................................................ 20
2.6.2
Temperature .................................................................................................. 20
2.6.3
Hysteresis of bias and drift ........................................................................... 21
2.6.4
Vibrations ...................................................................................................... 21
2.7
Kalman Filtering ............................................................................................... 21
ii
3
Design Procedure ....................................................................................................... 24
3.1
4
Description Of System ............................................................................................... 27
4.1
Position and Orientation Algorithm - Direction Cosine Matrices .................... 27
4.2
Earth rotational rate........................................................................................... 29
4.3
Inertial Measurement Unit ................................................................................ 31
4.4
Single State Non-Adaptive Kalman Filter and Integrator................................. 33
4.5
Triple/Dual State Adaptive Kalman Filter and Integrator ................................ 34
4.5.1
4.6
5
The adaptive Kalman filter ........................................................................... 37
Other Kalman Filter implementations .............................................................. 38
Experimental Procedure ............................................................................................. 45
5.1
Implementations of the DCM algorithm ........................................................... 46
5.1.1
Number Representation ................................................................................ 46
5.1.2
Generic Hardware Blocks ............................................................................. 47
5.1.3
Reconfigurable Hardware Implementation ................................................... 49
5.1.4
MSP430 Implementation .............................................................................. 49
5.2
6
Initial Designs ................................................................................................... 25
MATLAB simulations ...................................................................................... 50
5.2.1
Noiseless Trajectory...................................................................................... 50
5.2.2
Noisy Trajectory ........................................................................................... 51
5.2.3
Single State Kalman Filter Trajectory .......................................................... 51
5.2.4
Triple/Dual State Kalman Filter Trajectory .................................................. 52
5.2.5
Kalman Filter Initialization ........................................................................... 52
Results and Discussion ............................................................................................... 53
iii
6.1
IMU Simulation ................................................................................................ 53
6.2
Trajectories ....................................................................................................... 54
6.3
Filter tuning and Innovations term .................................................................... 57
6.4
No filtering vs. Non-Adaptive Filtering vs. Adaptive Filtering........................ 59
7
Future Work ............................................................................................................... 62
8
References .................................................................................................................. 64
9
Appendix .................................................................................................................... 65
9.1.1
MSP430 Implementation of the DCM algorithm ......................................... 65
LIST OF FIGURES
Figure 1.
METEOR Design Model - Concept Drawing and First Platform Prototype .. 2
Figure 2.
Rocket trajectory into Low Earth Orbit .......................................................... 3
Figure 3.
GPS satellite constellations – courtesy of www.space.com ........................... 6
Figure 4.
Pseudo-range to triangulate position – courtesy of www.wikipedia.org ........ 7
Figure 5.
Basic principle of an Inertial Navigation System ........................................... 7
Figure 6.
Gyroscope Concept Diagram – courtesy of www.wikipedia.org ................... 8
Figure 7.
Accelerometer array to sense translation and rotation .................................. 25
Figure 8.
Accelerometers sensing rotation around Z axis ............................................ 26
Figure 9.
Designation of axis on the METEOR Rocket ............................................... 28
Figure 10.
MEMSense IMU with labeled axes – courtesy of www.memsense.com . 31
Figure 11.
MEMSense IMU Data Packet – courtesy of www.memsense.com.......... 32
iv
Figure 12.
MEMSense IMU Data Packet Format – courtesy of www.memsense.com
33
Figure 13.
Integrator Block ........................................................................................ 47
Figure 14.
n-o-a-p Matrix Multiplier for DCM Algorithm ........................................ 48
Figure 15.
Reference and Noisy IMU Values ............................................................ 54
Figure 16.
Detailed Rocket Trajectories for Various Implementations ..................... 55
Figure 17.
3-D Rocket Trajectories for Various Implementations ............................. 56
Figure 18.
Z-Axis Displacement Towards the End of the Trajectory ........................ 57
Figure 19.
Z-Axis Acceleration Kalman Filter Terms During a System Change ...... 58
Figure 20.
Z-Axis Acceleration for Various Implementations During the Entire
Trajectory
59
Figure 21.
Z-Axis Acceleration for Various Implementations with an Invariant
System
60
Figure 22.
Z-Axis Acceleration for Various Implementations During a System
Change
61
Figure 23.
System Block Connections ....................................................................... 71
Figure 24.
Program Flowchart.................................................................................... 72
Figure 25.
Oscilloscope Screen for One Iteration ...................................................... 75
Figure 26.
Include Dependency Graph for app.c ....................................................... 77
Figure 27.
Call Graph for Function main() ................................................................ 78
Figure 28.
Call Graph for Function mat_mul_values() .............................................. 79
Figure 29.
Size of Resulting Build from the ELF file ................................................ 80
v
LIST OF SYMBOLS AND NOMENCLATURE
AKF –Adaptive Kalman Filter
ASIC – Application Specific Integrated Circuit
COTS – Commercial off-the-shelf
DCMs – Direction Cosine Matrices
FPGA – Field Programmable Gate Array
GPS – Global Positioning System
HDL – Hardware Description Language
IGS – Inertial Guidance System
IMU – Inertial Measurement Unit
INS – Inertial Navigation System
KF – Kalman Filter
MEMS – Micro-Machined Electro Mechanical Systems
vi
1
INTRODUCTION
In the past century, methods of transportation have changed drastically. Every day, the
world demands more efficient, faster and more accurate ways of transporting people and
goods.
Navigation has therefore become an important area of study.
Navigation
provides the means for an accurate transportation. Over time, different navigation tools
have been developed, most recent by taking advantage of microelectronics and
miniaturization. GPS is the most popular tool being used today for navigation. GPS
provides the user with accurate position information from anywhere around the Earth.
This system uses synchronized satellite signals to triangulate the GPS receiver’s position,
achieving an accuracy of around 10 meters. This information is typically updated every
second. GPS receivers have become inexpensive, and so they are used in many different
applications in which the 1Hz rate is fast enough. Other applications require more
accurate information at a much faster rate. In particular, high speed missiles and rockets
use position information in a control feedback loop to steer them on a predetermined
path. In these cases, the navigation system needs to accurately keep track of the position
and orientation at a much faster rate than in other applications.
Inertial navigation is the most popular solution to this accuracy problem.
Inertial
navigation uses inertial sensors, such as accelerometers and gyroscopes, in order to
determine the movements of the body to which they are attached. If the sensors are
accurate and fast enough, minute movements can be detected and used in inertial
calculations.
1
1.1 The METEOR Project at RIT
The METEOR project is a multi-disciplinary, multi-year research project whose ultimate
goal is the application of micro-technology to space exploration. The project is the first
step in an ambitious plan to get RIT involved in a student led space program. There are 3
major components that comprise this project. All the components in this project work
together in achieving critical tasks, which are indispensable for the success of a mission.
The first component is a high altitude platform capable of deploying and launching a
rocket, as seen in the figure below.
Figure 1.
METEOR Design Model - Concept Drawing and First Platform Prototype
The platform travels to an altitude of about 30 kilometers utilizing helium filled highaltitude balloons, and after performing the necessary mission tasks, returns to Earth using
2
a parachute. The second component is a four-stage rocket that carries a small 1kg
satellite. After ignition, it travels to low-earth orbit and finally its fourth stage reaches
orbital speed, releasing the satellite. The figure below shows the trajectories of the
platform and the rocket into low Earth orbit. The third and final component is the
satellite. It is envisioned that this method of deployment will be more economical for this
class of satellites.
Figure 2.
Rocket trajectory into Low Earth Orbit
The scope of this graduate study is to analyze and design a navigation tool for this rocket.
In order to keep track of the rocket’s current position, an inertial dead reckoning (short
for deduced reckoning) system is used.
The dead-reckoning system could be
implemented either in a processor based computer or in a in a reconfigurable hardware
device such as an FPGA.
It receives data from a commercially available IMU
(acceleration, rotational rate, and depending on make and model, magnetometer
information), and determines the current position and attitude with respect to an initial
3
reference system. The system determines the new orientation and position by using
direction cosine matrices to describe a frame transformation (rotation and translation)
between samples. This information can then be used to navigate the rocket through a
predetermined trajectory “tunnel”, using a feedback control system. Since the system
does not intend to match the orbital insertion accuracy of commercial launchers, a GPS is
not used. Another reason that the GPS is not used is that GPS receivers have operational
restrictions at high altitudes and speeds.
2
REVIEW OF LITERATURE
2.1 Inertial Navigation Systems
An inertial navigation system is used to determine a body’s position with respect to a
known reference. Inertial navigation systems are most commonly used in conjunction
with Inertial Guidance Systems in order to guide launch vehicles and projectiles. The
main elements of an inertial navigation system are an Inertial Measurement Unit (IMU),
and a computer which uses the information from the IMU to calculate the current
position. IMU’s typically contain 3 accelerometers and 3 gyroscopes (one pair on each
orthogonal axis). There are many types of IMU’s available commercially, which use
different types of inertial sensors, as we will see in the next sections. There are also
many different ways to implement the computer which calculates the position. Recently,
DSP’s have been the solution of choice due to high clocking rates and specialized
instruction sets which speed up computations. Improved computation capabilities of
newer DSP’s have allowed engineers to design and implement complex algorithms that
4
increase the performance of the INS’s. Because of this, and because of more accurate
and stable inertial sensors, inertial navigation has come a long way over the past 50 years.
INS’s have also been improved with the introduction of GPS. Using a Kalman filter,
which “fuses” information from various sources (in this case GPS and IMU), hybrid
INS’s can achieve long term and short term accuracy. The GPS subsystem can only
obtain position fixes every second, and they are accurate to about 10 meters. Purely
inertial navigation on the other hand, can obtain shorter term accuracy, but bias drift in
the sensors would cause a “random walk” in the position calculations over a long time,
and thus need a periodic “fix” such as the one GPS provides.
Inertial Navigation Systems are being developed and used mainly for aerospace and
military applications due to the high cost of the systems. High precision components are
incorporated into the IMU to produce good results. The most critical are the gyros, which
are used to determine the angular rate of rotation or the orientation of the system.
Knowing the correct orientation of a system in 3-dimensional navigation is critical, since
we have to properly account for gravity in the down direction. If gravity is accounted for
in the accelerometer readings using inaccurate orientation calculations, then the system
will think it is moving in the wrong direction. Also high quality accelerometers are
important. Ideally there should be no bias (or very low at least) in the sensors, and they
should behave in a simple linear fashion. The presence of noise, disturbances, drifts,
misalignments, and manufacturing flaws make the development and implementation of
an INS difficult.
5
2.2 GPS Network
The GPS network of satellites contains a minimum of 21 satellites that orbit the Earth at
an altitude of about 11,000 nautical miles (20,000 kilometers). They orbit once every 11
hours and 58 minutes, so that they drift in the sky 4 minutes a day (they drift 2 minutes
each orbit). There are 6 different orbits that the satellites can be in, with multiple
satellites in each orbit. Each possible orbit is inclined 55 degrees with the equator with no
orbits going directly over the poles, as shown in the figure below.
Figure 3.
GPS satellite constellations – courtesy of www.space.com
Finding your location using GPS is accomplished by triangulating your position from the
known positions of GPS satellites. Distance measured from your position to the satellite
is measured by time of flight of a signal sent from the GPS satellite and received by the
receiver. The signal sent from the satellite tells its location and the time the signal was
sent. Since the signal travels at the speed of light, the distance from the receiver to the
satellite can be calculated. With a GPS receiver and one satellite, you can determine your
6
possible location on a sphere with the satellite at the center. Unfortunately this is not very
useful. With two satellites we get two spheres that are centered at different locations.
Now our position lies at the intersection of these two spheres, which is a circle.
Unfortunately this is still not useful. If we use a third satellite, and introduce a third
sphere, we get two possible locations for the receiver in 3D space, as shown in the figure
below.
Pseudo-range to triangulate position – courtesy of www.wikipedia.org
Figure 4.
Immediately one of the two possible solutions can be discarded, leaving us with our
position. Thus with three satellites, we can easily determine our position in 3D space.
The figure below contains the basic system block diagram of an INS aided system, where
more than one motion sensor sub-system exists, and corrections are made according to a
filtering method.
Figure 5.
Basic principle of an Inertial Navigation System
7
2.3 Inertial Measurement Units
Inertial Measurement Units (IMUs) typically have 3 accelerometers and 3 gyroscopes,
each pair affixed to each 3D orthogonal axis. An accelerometer measures accelerations
(forces) along the axis to which it is affixed, while a gyroscope measures rotational rates
around the axis to which it is affixed.
A combination of 3 accelerometers and 3
gyroscopes affixed to 3 orthogonal axes on a vehicle provide enough information to track
its position and orientation.
2.3.1
Gimbaled
The first type of INS developed was a gimbaled system. In a gimbaled system, the
accelerometers are mounted on a motorized gimbaled platform which is always kept
aligned with the navigation frame. Pickups are located on the outer and inner gimbals
which keep track of the attitude of the stabilized platform relative to the vehicle on which
the INS is mounted.
Figure 6.
Gyroscope Concept Diagram – courtesy of www.wikipedia.org
8
This setup has several disadvantages:

Bearings are not frictionless.

Motors are not perfect (i.e. dead zones, etc.).

Consumes power to keep the platform aligned with the navigational frame which
is not always good on an embedded system.

Cost is high due to the need for high quality motors, slip rings, bearings, and other
mechanical parts. Thus such systems were mainly for military uses on planes,
ships, and intercontinental ballistic missiles.

Recalibration is difficult, and requires regular maintenance by certified personnel
which could be difficult on an autonomous vehicle. Plus any maintenance that
must be performed on the system (i.e. replace bearings, motors, etc.) must be done
in a clean room and then the system must go through a lengthy recertification
process.

Gimbal lock
Gimbal lock is an effect which prevents a two-degrees-of-freedom gyroscope having
360° of freedom about both its inner and outer gimbal axes. Gimbal lock occurs when the
spin axis of the rotor coincides with the outer gimbal axis owing to a 90° rotation about
the inner axis. At this point, the gyroscope loses one degree of freedom. Application of
motion about an axis perpendicular to the plane containing the outer gimbal causes the
outer gimbal to spin. Once this spinning motion has begun, the spin axis of the rotor and
the axis of the outer gimbal remain permanently coaxial. The only method of separating
them is to stop the rotation of the inertial element to allow the two axes to be reset. This
9
undesirable effect is prevented by using mechanical stops to limit the motion of the inner
gimbal. These stops usually permit up to ±85° of motion by the inner gimbal. Gimbal
lock can also occur in stable platforms with three gimbals.
2.3.2
Strapdown
A strap-down system is a major hardware simplification of the classic gimbaled systems.
The accelerometers and gyros are mounted in body coordinates and are not mechanically
moved. Instead, a software solution is used to keep track of the orientation of the IMU
(and vehicle) and rotate the measurements from the body frame to the navigational frame.
This method overcomes the problems encountered with the gimbaled system, and most
importantly reduces the size, cost, power consumption, and complexity of the system.
2.3.3
MEMS Inertial Sensors
New applications that have demanded low-cost sensors for providing measurements of
acceleration and angular motion have provided a major incentive for the development of
micro-machined electromechanical system (MEMS) sensors. MEMS devices are one of
the most exciting developments in inertial sensors in the last 25 years. These devices
overcome many of the problems of previous technologies, especially cost, size and power
consumption. Many efforts have been applied to the design and manufacture of
conventional inertial instruments, with significant success; however, the cost has
remained high. The major reasons have been high parts count, requirement for parts with
high-precision tolerances, complicated and precise assembly techniques, and accurate
testing, characterization and calibration.
10
The use of silicon as the base material in the manufacture of the components offers a
radical approach and overcomes many of the issues considered above for conventional
mechanical sensors. Additionally, since they can be integrated with other sensors on a
chip, they have the capability to make an integrated precision multi-sensor inertial
navigation system (for example with GPS aiding) costing much less than the equivalent
conventional system.
Number of Parts
125
70
44
30
5
3
1955: Floated Gyros
1965: Tuned Gyros
1975: Ring laser Gyros
1985: Fiber optic Gyros
1990: Vibratory Gyros
2000: MEMS Gyros
Table 1: Gyros and Number of parts throughout the years
In general, MEMS devices have the following advantages:

small size

low weight

rugged construction

low power consumption

short start-up time

inexpensive to produce (in high volume)

high reliability

low maintenance

compatible with operation in hostile environments
11
Despite these advantages, due to the reduced size, sensitivity and noise immunity are
smaller. In addition, there are thermal sensitivity concerns. Because of the smaller size
and mass, the sensors are much more prone to bias effects due to temperature changes.
Nevertheless, low cost MEMS gyroscopes and accelerometers demonstrate performance
approaching l°/h and 50-100 micro-G respectively. One of the major reasons for the
enhanced performance of MEMS devices is the ability to compensate systematic errors
by having detailed knowledge of both the error mechanisms and the fundamental
characteristics of each sensor type, and use of a Kalman filter to compensate for these
errors.
Ease of system integration is another advantage that makes MEMS inertial sensors
attractive for many applications. An array of MEMS sensors may be integrated into a
single chip to provide multiple independent measurements of inertial motion. A direct
advantage of this class of sensors is that they may be integrated with the electronic
control circuits in an applications specific integrated circuit (ASIC). MEMS devices are
ideal for integration with other navigation systems, both in terms of complementary error
characteristics and small size with rugged operational characteristics.
2.4 Direction Cosine Matrices
Most modern orientation sensing systems use a DCM to calculate and present data. This
includes rockets, commercial airliners, and current unmanned aerial vehicle (UAV)
systems. Their features are:
12

Contain no singularities

No linear interpolation over rotation space

Require orthonormalization to ensure proper rotations (complex operation)

Easy to transform many vectors between different frames

Many elements for math operations and filtering
Direction cosine matrices can be used to define a 3-dimensional frame orientation or
frame transformation. The transformation can include rotations and translations about all
3 axes. An initial frame consists of the orientation and position of the body in the
following manner:
Initial = [
nx,
ox,
ax,
px;
ny,
oy,
ay,
py;
nz,
oz,
az,
pz;
0,
0,
0,
1
];
The n-o-a vectors are unit vectors, and they are orthogonal to each other. They describe
the orientation of the body with respect to a reference frame. The letters ‘n’, ‘o’, and ‘a’
stand for normal, orientation and approach, respectively. The ‘p’ vector describes the
position of the body with respect to the reference frame.
To describe a transformation, we use the following matrices. These matrices, when post
or pre-multiplied with the current frame, will result in a new frame after the
transformation is applied.
13
Trans_xyz = [ 1,
0,
0,
dX;
0,
1,
0,
dY;
0,
0,
1,
dZ;
0,
0,
0,
1
Rot_x =
[ 1,
0,
Rot_z =
0,
0;
0,
cos(φ), -sin(φ),
0;
0,
sin(φ), cos(φ),
0;
0,
Rot_y =
];
0,
0,
1
];
[ cos(θ),
0,
sin(θ,
0;
0,
1,
0,
0;
-sin(θ),
0,
cos(θ,
0;
0,
0,
0,
1
[ cos(ψ), -sin(ψ), 0,
0;
sin(ψ), cos(ψ),
0,
0;
0,
0,
1,
0;
0,
0,
0,
1
];
];
When a transformation is described by two or more of the basic transformations (we
might first rotate around the x-axis, then translate on the resulting z-axis, and then rotate
about the y-axis), we can multiply the basic transformation matrices together to get one
transformation matrix that describes the complete transformation. If the transformation
matrix is left-multiplied to the current frame, then the transformation is done with respect
14
to the original frame. If the transformation matrix is right-multiplied to the current frame,
then the transformation is done with respect to the last resulting frame.
2.5 Quaternions
Although quaternions are an old numbering system relative to the procedures referenced
here, their use in these procedures has only recently been evaluated. After the widespread
adoption of vectors and vector fields (which are derived from quaternions) in most
aspects of physics, quaternions, with their more complicated algebra and calculus, went
out of favor with physicists and engineers. Their features are:

Contain no singularities

Linear interpolation exists over rotation space

Require normalization to ensure proper rotations (simple operation)

Minimal non-singular representation of orientation

Filtering exists entirely in quaternion space on the surface of the 4-D hypersphere
Quaternions were discovered in 1843 by William Hamilton as a way to multiply triples of
numbers. Quaternions are a complex number system with three distinct imaginary parts
(i, j, k) given by the equation
q  w  ix  jy  kz
w, x, y, z  
where the complex parts are defied as
i 2  j 2  k 2  ijk  1
The set of quaternions is denoted by the symbol H, and the identity quaternion is (1, 0, 0,
0). A quaternion can also be represented as a scalar s and a vector v defined by
15


q  s, v   s  v  iˆ, ˆj, kˆ
The norm of a quaternion is its length in the complex 4-space, defined as
q  w2  x 2  y 2  z 2
The scalar product of two quaternions a and b is defined as
a  b  wa wb  xa xb  y a yb  z a z b
The conjugate of a quaternion q = (s, v) is

q '  ( s,v )
and the inverse of q is
q 1 
2.5.1
q'
q
Quaternion rotation
For a unit quaternion, the inverse is equal to the conjugate. A subset of the quaternions is
the Unit Quaternion, denoted by H1. For a unit quaternion, one which lines on the
surface of a unit 4-D hypersphere, |q| = 1. These quaternions have a simple relationship
to the set of rotations in 3-space, named SO(3), defined by
       
q   cos , n sin   
 2 
  2
where n is a unit 3-vector of the axis of rotation and θ is the angle to rotate around n by
the right-hand-rule. Note that a rotation defined by this method has a single redundancy
that q = -q when used as a rotation.
16
Rotating an orientation quaternion using a rotation quaternion is a simple operation
defined by
prot  qpq'
where p is the original quaternion, prot is the rotated quaternion, and q is the quaternion
that defines the rotation.
Rotating a vector using a rotation quaternion is an almost identical operation
n, vrot   q0, v q'
where v is the original vector, vrot is the rotated vector, and q is the quaternion that
defines the rotation. To perform two sequential rotations, one around ‘a’ and then around
‘b’, the rotation q is constructed by q = b·a, and then the operation is performed.
The orthogonal matrix corresponding to a rotation by the unit quaternion z = a + bi + cj +
dk (with |z| = 1) is given by
2.5.2
Conversions from quaternions to DCMs
Since spatial rotations in three dimensions can be parameterized using both unit
quaternions and Euler angles, it is intuitive that we can convert from one to the other.
A unit quaternion can be described as:
17
where α is a simple rotation angle and and cos(βx), cos(βy) and cos(βz) are the "direction
cosines" locating the axis of rotation (Euler's Theorem).
The orthogonal matrix corresponding to a rotation by the unit quaternion q is given by
The orthogonal matrix corresponding to a rotation with Euler angles
By comparing the terms in the two matrices, we get
18
, is given by
For Euler angles we get:
Dual quaternions can also be used to parameterize a rotation and a translation at once,
rather than first rotating, and then translating based on the results of that rotation. Dual
quaternions representation simultaneously solves for orientation and position estimate by
minimizing a single cost function associated with the sum of orientation and position
errors, rather than estimating the translation based on the estimation of the rotation.
2.5.3
Choosing between quaternions and DCMs
The quaternion method does offer some advantage because, inherently, it gives rise to an
orthogonal attitude matrix. Also, the accuracy of attitude computation obtained using
quaternions is superior to the accuracy which may be achieved using the DCM’s.
However, the selection of attitude algorithms is unlikely to be made solely based on
computational accuracy. For the work done in this study, it was the computational
intensity and the memory requirements that were the major factors in choosing DCM’s
over quaternions for this particular application.
2.6 Sources of Error
The sensors that are in the IMU introduce several errors that will be explained in the
following sections.
19
2.6.1
Bias and Drift
These are the most devastating effectors on accuracy to an IMU. Drift rate for the gyros
and accelerometer bias are small offsets which the IMU incorrectly reads, that must be
properly accounted for. The bias has a quadratic effect on the position derived from the
IMU.
Bias (m/s^2)
0.1
0.01
0.001
0.0001
Error (m)
t=100 sec
Error (m)
t = 30 mins
50
162000
5
16200
0.5
1620
0.05
162
Table 2: Positional error from biases after 100 seconds and 30 minutes
Looking at the table above, it becomes apparent that determining the bias is of critical
importance if any accurate measurement is expected. The drift rate has a similar and
equally massive impact on the position of a system. If a drift is not properly accounted
for, and the IMU thinks it is rotating, then the navigation equations will not properly
account for gravity and the system will think it is moving due to a maximum acceleration
of 9.8 depending on how far the system has drifted.
2.6.2
Temperature
The IMU’s accelerometers and gyros are sensitive to temperature.
Thus as the
temperature of the IMU changes, the associated bias and drift will change until the
temperature reaches steady state or remains the same. This is critical in our application,
since the system is expected to be subject to extreme changes in temperatures. Many
IMU contain a temperature sensor, and use it to compensate their outputs.
20
2.6.3
Hysteresis of bias and drift
The drift rates and accelerometer biases tend to change each time the unit is switched on.
This is due to the fact that measurements are noisy due to intrinsic sensor properties.
Typically there is a low pass filter used to remove some of this noise before the
measurements are used in the navigation equations (also realistically, there tends to be
low pass filtering somewhere in the system due to hardware limitation because not
everything has infinite bandwidth). When random noise is filtered, this produces what is
called a random walk. The integration of this random walk will result in velocity and
positions moving at different rates during different runs even though the IMU (and
vehicle) are in the same orientation and experiencing the same accelerations during each
run.
2.6.4
Vibrations
Vibration in a strap-down system can cause many problems with the INS. Generally great
care must be taken to isolate the IMU from any resonance frequencies. In high precision
systems, various tests must be done to try to identify what these frequencies are then
design elaborate mounts to hold the IMU.
2.7 Kalman Filtering
The problem with having a purely inertial navigation system is that since we are
measuring accelerations and rotational rates, then the error in the final position due to
sensor errors grows with t3. These errors eventually lead to a random walk, which is
otherwise unavoidable, and can have detrimental effects on the precision of the system.
21
A Kalman filter is a computational algorithm that processes measurements to deduce a
minimum variance, unbiased error estimate of the state of a system by utilizing the
following: knowledge of the system and measurements dynamics, assumed statistics of
system noises and measurement errors, and initial condition information.
The main idea behind the Kalman filter is that we use various sources of noisy (white
Gaussian) information in order to calculate a better estimate of what the sources are
trying to tell us.
In the case of this inertial system, if only the acceleration and
gyroscopic motion information is used, there is no redundancy. If we add other sensors
to the system, we can then create a system model that relates the new information we
obtain from the new sensors, to the information obtained from the accelerometers and
gyroscopes. Using this system model, we can then have a weighted mean on the two
sources of information, and thus come up with a better estimate of the “true” position of
the system. This is basically what the Kalman filter achieves.
As a simple example, if we are measuring the position and velocity of an object traveling
with no acceleration (using 2 different sensors), we first want to obtain an estimate of the
system variables, and then obtain the measurements from the sensors, which are related
to each other by a system model. The estimates of the position and velocity will be
denoted by x and x .
The estimate Xi+1|i denotes the estimate at time i+1 using
information up to time i.
 x
1 t   x 

 x 

 
  i 1|i 0 1   x  i|i
The 4x4 matrix above is called the state transition matrix (normally denoted by Φ), and
explains how the state variables change from one state to the next.
22
The measurements will be denoted by y and y .
 y
 y 
  i 1
Using the estimates and the measurements, we can use a weighted mean to come up with
the best estimate of the state variables up to time i+1:
X i 1|i 1  1  K X i 1|i  KYi 1
The Kalman gain K can be constant, but if the measurements start to become noisier,
there would be no way of adapting the filter to adjust to the noise. If K is variable, we
can adapt the filter to the current noise conditions.
Let’s say now, that the object can have a constant acceleration. The state transition
matrix and Xi would then become:

1 t

 i  0 1
0 0

t 2 

2 
t 
1 

i
 x
X i   x 
 x i
Since we only have measurements of position and velocity, we need to derive a way of
calculating the acceleration from these two measurements. For example, we can take
differences of the best estimates of velocities. When we can’t measure values of the state
variables directly, we use what Kalman called the measurement matrix H. H converts the
state variable data into equivalent values of measured data. In our case, Y has the same
units as HX. Now, to obtain the new estimates we first do the prediction and then use it
to get the correction in the following way:
23
X i 1|i   i X i|i
X i 1|i 1  X i 1|i  K Yi 1  H i 1 X i 1|i 
If the state variables can be directly measured, then the H matrix is the identity matrix.
The Kalman gain depends on the variances of the measurements and the estimates:

K i 1|i 1  Pi 1|i H i 1 H i 1 Pi 1|i H i 1  Ri 1
T
T

1
Where Pi is the variance of the estimates, and Ri is the variance of the measurements. To
obtain the prediction variance of the estimates, we can use the following:
Pi 1|i  Pi|i T  Qi
where Qi is the system noise matrix, which is an estimate of the error in how we modeled
the system.
Then we can calculate the correction variance Pi+1|i+1:
Pi 1|i 1  I  K i 1 H i 1 Pi 1|i
There is a small problem with inertial navigation and Kalman filtering. Kalman filtering
as it has been explained here, only works with linear systems (Extended Kalman Filters
are much more complex, but can be used with non-linear systems). Inertial navigation is
non-linear, but by studying the system, and making some assumptions and
approximations, we can make the navigation problem linear.
3
DESIGN PROCEDURE
This section describes the initial design ideas that were studied, and how and why they
developed into what is now the final design.
24
3.1 Initial Designs
The initial idea for this study was to develop a dead-reckoning system for the METEOR
Rocket using only strapdown MEMS accelerometers. The reason for using a strapdown
MEMS inertial sensor is that this technology is much more power efficient and easier to
embed than a gimbaled device. MEMS accelerometers have been around for some years
now, and they prove to be robust and accurate. Since many of the commercial and
military grade IMU’s in the market today use either ring laser gyros or fiber optic gyros,
the initial design involved using only accelerometers to determine rotational and
translational movements of the rocket.
Accelerometers can be used to determine gyroscopic motion (rotation around an axis) by
using a sensor fixed at a certain known distance from the axis of rotation and measuring
the acceleration exerted on the sensor about the axis through the sensor, tangent to the
rotation. The figure below shows a series of accelerometers fixed to a cylindrical body in
a certain array in order to sense translation and rotation about all three axes.
Figure 7.
Accelerometer array to sense translation and rotation
25
One of the problems that was encountered with this design is that the distance between
the accelerometer pair designated to measure rotations about an axis needs to be quite
large, because the sensitivity of the accelerometers is not high enough to accurately
measure very slow rotations if the separation is small (say 50cm – which is the expected
diameter of the rocket). If a small diameter was used between accelerometers, only high
rotational rates would be sensed as useful information, and smaller rotational rates would
have too much noise to determine the true rate of rotation. Also, the computational
requirements to calculate the tri-axial rotational rates from a network of accelerometers
would be far much complex than simply using a gyro reading.
Figure 8.
Accelerometers sensing rotation around Z axis
26
4
DESCRIPTION OF SYSTEM
4.1 Position and Orientation Algorithm - Direction Cosine Matrices
The current system involves a dead-reckoning system that uses a COTS IMU with a triaxial accelerometer and a tri-axel gyroscope to determine the movement of the body to
which it is affixed. The values from the IMU are fed into a Kalman filter to remove (to a
certain extent) any white Gaussian noise that might be present in the data from the IMU.
The system captures samples from the IMU at a rate of 100Hz, processes them in the
Kalman filter, and then computes the current position and orientation frame using
direction cosine matrix transformations in the following way:
DIFF_Frame = Trans_xyz * Rot_x * Rot_y * Rot_z;
NEW_Frame = OLD_Frame * DIFF_Frame;
By multiplying the transformation matrices in that order, we are first translating with
respect to OLD_Frame, and then rotating around the x axis of the resulting frame of the
previous translation, then rotating around the y axis of the resulting frame of the previous
rotation, and finally rotating around the z axis of the resulting frame of the previous
rotation.
27
Using this method, only the current frame and the difference frame (transformation
frame) are needed in order to compute the new frame. Any initial frame is not needed, as
it would be if we were to left multiply the transformation frame.
This method makes use of the “small-angle” assumption, because if we look at a
transformation that rotates around x, then y and finally z, and we are changing the current
frame after every rotation (right multiplying), the original y axis will differ with the
current y axis after the x axis rotation. The same happens with the original and current z
axes after the y axis rotation. If we assume small x-axis rotations, and small y-axis
rotations, as is the case for a rocket, then the changes in the original and current y and z
axes are small. The last rotation does not need to be a small angle, since the order in
which we right multiply the rotation matrices is x, then y, and finally z. If we place the
body frame onto the rocket, so that the z axis is along the propulsion axis as shown in the
figure below, then large rotations on the x and y axes are not likely. Large rotations in
the z axis might be likely but do not matter (rocket spinning around its propulsion axis).
Figure 9.
Designation of axis on the METEOR Rocket
28
In order to facilitate the use of this algorithm in hardware, the matrix multiplication was
carried out, and a generic function was created that would generate the new n-o-a-p
matrix. The expanded terms of the resulting n-o-a-p matrix are shown below, where phi
is the rotation about the x-axis, theta is the rotation about the y axis and psi is the rotation
around the z axis:
nx_temp = nx*cosTHETA*cosPSI + ox*(+sinPHI*sinTHETA*cosPSI+cosPHI*sinPSI) + ax*(-cosPHI*sinTHETA*cosPSI+sinPHI*sinPSI) + 0;
ny_temp = ny*cosTHETA*cosPSI + oy*(+sinPHI*sinTHETA*cosPSI+cosPHI*sinPSI) + ay*(-cosPHI*sinTHETA*cosPSI+sinPHI*sinPSI) + 0;
nz_temp = nz*cosTHETA*cosPSI + oz*(+sinPHI*sinTHETA*cosPSI+cosPHI*sinPSI) + az*(-cosPHI*sinTHETA*cosPSI+sinPHI*sinPSI) + 0;
ox_temp = nx*cosTHETA*sinPSI + ox*(-sinPHI*sinTHETA*sinPSI+cosPHI*cosPSI) + ax*(+cosPHI*sinTHETA*sinPSI+sinPHI*cosPSI) + 0;
oy_temp = ny*cosTHETA*sinPSI + oy*(-sinPHI*sinTHETA*sinPSI+cosPHI*cosPSI) + ay*(+cosPHI*sinTHETA*sinPSI+sinPHI*cosPSI) + 0;
oz_temp = nz*cosTHETA*sinPSI + oz*(-sinPHI*sinTHETA*sinPSI+cosPHI*cosPSI) + az*(+cosPHI*sinTHETA*sinPSI+sinPHI*cosPSI) + 0;
ax_temp = nx*sinTHETA
+ ox*(-sinPHI*cosTHETA
)
+ ax*(+cosPHI*cosTHETA
)
+ 0;
ay_temp = ny*sinTHETA
+ oy*(-sinPHI*cosTHETA
)
+ ay*(+cosPHI*cosTHETA
)
+ 0;
az_temp = nz*sinTHETA
+ oz*(-sinPHI*cosTHETA
)
+ az*(+cosPHI*cosTHETA
)
+ 0;
px_temp = nx*dX
+ ox*dY
+ ax*dZ
+ px;
py_temp = ny*dX
+ oy*dY
+ ay*dZ
+ py;
pz_temp = nz*dX
+ oz*dY
+ az*dZ
+ pz;
As we can see, the general form of the terms is:
z  n  t1  t 2   o  t 3  t 4   t 5  t 6  t 7   a  t8  t 9   t10  t11  t12   p
where ti is the ith term from the equations above.
We can implement this generic function as a software function (for processor
implementation) or as a hardware block (for reconfigurable hardware). The function
utilizes 10 multipliers and 5 adders.
4.2 Earth rotational rate
Inertial Navigation is governed by two vector differential equations:
29


  
 
v  a  g  2    xv
 
p  v
where a is acceleration along the axis, v is velocity along the axis, g is the gravitational
force along the axis, and the other variables are described below.
From these equations we can see that the derivative of position is simply velocity, and the
derivative of velocity is the acceleration of the vehicle (a+g) minus the Coriolis
correction. The Coriolis correction is made up of two terms, the correction due to Earth’s
rotation, and the correction due to the rotation of the navigation coordinate system with
respect to inertial space. These two terms are defined as:
cos  

    0 
 sin  
1

r
vE
 vN
 v E tan 
where
where
  Earths _ rotation _ rate
  latitude _ of _ vehicle _ position
r  radius _ of _ Earth
v  velocity _ of _ vehicle
  latitude _ of _ vehicle _ position
Since the IMU that will be used is a low cost IMU, and we do not need to achieve a high
orbital insertion accuracy, we assume that the rotation rate of the Earth and the navigation
coordinate system is zero. The magnitude of the Earth’s rotation is 7.27E-5 rad/s, which
means that an IMU with a standard deviation error in the gyros (in rad/s) higher than this
will not be able to sense the rotation of the earth. As an example, the MEMSense IMU
gyros have an uncertainty of 8.7E-3 rad/s, which is quite higher than 7.25E-5 rad/s.
Therefore, much inaccuracy is introduced from noisy IMU measurements than from
disregarding these terms.
30
4.3 Inertial Measurement Unit
The current solution uses a MEMSense uIMU inertial measurement unit. This IMU
provides serial digital outputs of tri-axial acceleration, rate of turn (gyro) and tri-axial
magnetic field data. The IMU has a rugged aluminum housing. Digital outputs are
configured to the RS422 protocol. Correction algorithms provide high performance,
temperature compensated, tri-axial inertial data in real time. The µIMU is available in a
custom package measuring 2.2 in. diameter × 0.8 in. height (roughly the size of a 9 volt
battery). The IMU weighs 150 grams, needs a 9 to 13 volt supply, and consumes about
180 mA of current.
Figure 10.
MEMSense IMU with labeled axes – courtesy of www.memsense.com
The range of the accelerometers is ±50G on two axes and ±5G on the third. The range of
the gyroscopes is ±1200deg/sec on all 3 axes. The range of the magnetometers is
±1.9Gauss on all 3 axes.
The byte order format that is obtained from the RS422
formatted serial output is shown below:
31
Figure 11.
MEMSense IMU Data Packet – courtesy of www.memsense.com
A complete sample structure is shown below:
32
Figure 12.
MEMSense IMU Data Packet Format – courtesy of www.memsense.com
4.4 Single State Non-Adaptive Kalman Filter and Integrator
In the single state design, there were actually 6 separate filters implemented, one for each
accelerometer and one for each gyro. For the accelerometers, only the acceleration was
estimated, and thus it was this estimate that was integrated twice to obtain the
displacement in every iteration, and that was fed into the DCM algorithm. As for the
gyros, only the gyro rate was estimated, and it was this estimate that was integrated once,
and then fed into the DCM algorithm. The state transition matrix was basically just a 1x1
identity matrix (just the number 1).
 i  1i
X i  xi
Using the estimates and the measurements, we used a weighted mean to come up with the
best estimate of the state variable up to time i+1:
X i 1  1  K X i  KYi 1
33
Since the state variable can be directly measured, the H matrix is the identity matrix
(again, just the number 1).
The Kalman gain depends on the variances of the measurements and the estimates:
Ki 
Pi
Pi  Ri 1
where Pi is the variance of the estimates, and Ri is the variance of the measurements. To
obtain the variance of the estimates, we can use the following:
Pi 1  1  K Pi
4.5 Triple/Dual State Adaptive Kalman Filter and Integrator
The triple/dual state adaptive Kalman filter is a little more complex than the single state
filter, but the basic functionality and concept behind it is the same. Here, we implement
6 separate filters again, but this time, for each accelerometer we have 3 states (change in
position, velocity and acceleration), and for each gyro we have 2 states (change in
rotational position and rotational rate). We want to obtain an estimate for change in
position and change in rotational rate, so that the integration is handled by the Kalman
estimation (less error than integrating an estimate), and also because this information can
be fed directly into the DCM algorithm without further calculations or formatting.
We used a constant acceleration model for the accelerometer filter, since the thrust from
the rocket is expected to be constant. Therefore, the state transition matrix and Xi then
become:
34

0 t

 i  0 1
0 0

t 2 

2 
t 
1 

i
 x
X i   x 
 x i
Notice that the first element of the state transition matrix is zero; this means that the first
state describes the displacement for the current iteration, not overall displacement since
startup. Since we only have measurements of acceleration, we need to derive a way of
calculating the displacement and velocity from this measurement. This makes the H
matrix H = [0 0 1]. Now, to obtain the new estimates we use the following:
X i 1|i 1  X i 1|i  K Yi 1  H i 1 X i 1|i 
The Kalman gain depends on the variances of the measurements and the estimates:

K i 1|i 1  Pi 1|i H i 1 H i 1 Pi 1|i H i 1  Ri 1
T
T

1
where Pi is the variance of the estimates, and Ri is the variance of the measurements. To
obtain the variance of the estimates, we can use the following:
Pi 1|i  Pi|i T  Qi
where Qi is the system noise matrix, which is an estimate of the error in how we modeled
the system.
The Q matrix used is:
 4

 43

Qi  q 
2
 2

2
3
2
2

35
2

2
 


1

where q is the process noise value.
The value of q represents the un-modeled
acceleration acting on the target and is chosen to minimize a cost function representing
the error in the target state.
The system noise matrix basically controls how much the system trusts the measurements
(ultimately controls the Kalman gain K). In order to make the filter adaptive, we need a
way to change this matrix when the system changes. This is explained in detail in the
next section.
Then we can calculate Pi+1|i+1:
Pi 1|i 1  I  K i 1 H i 1 Pi 1|i
As for the gyro filters, we used a constant rotational rate filter. Therefore, the state
transition matrix and Xi then become:
1 t 
i  

0 1  i
 rotation 
Xi  

rotation _ rate i
Since we only have measurements of rotational rate, we need to derive a way of
calculating the rotation from this measurement. This makes the H matrix H = [0 1].
Now, to obtain the new estimates again we use the following:
X i 1|i 1  X i 1|i  K Yi 1  H i 1 X i 1|i 
The Kalman gain again depends on the variances of the measurements and the estimates:

K i 1|i 1  Pi 1|i H i 1 H i 1 Pi 1|i H i 1  Ri 1
T
T

1
where Pi is the variance of the estimates, and Ri is the variance of the measurements. To
obtain the variance of the estimates, again we can use the following:
Pi 1|i  Pi|i T  Qi
36
where Qi is the system noise matrix, which is an estimate of the error in how we modeled
the system.
The Q matrix used is:
 2  
Qi  q 

  1
where q is the process noise value.
Then we can calculate Pi+1|i+1:
Pi 1|i 1  I  K i 1 H i 1 Pi 1|i
4.5.1
The adaptive Kalman filter
Suppose that when tracking the rocket, it suddenly starts turning (following a
predetermined trajectory). In this case, the system unexpectedly (according to the filter)
changed. This change could be either unpredictable noise or the system model becoming
mismatched from reality. In these cases, a method of changing the K matrix to the new
system is needed. In this adaptive Kalman filter, the values of the Q matrix are increased
in times of greater uncertainty. The difference between the current measurement and the
current state (Y-HX) is called the innovations term. By looking at the innovations term,
we can determine if there is too much unexpected noise or if the system has become
mismatched, and thus we can change the Q matrix accordingly. The innovations term
should have a distribution that approximates to white noise with a small variance. If we
monitor the innovations term, and we see that the variance of the terms went up, we can
increase the system noise matrix. The only problem is that once the Q matrix values are
increased, the filter gives poorer results, and thus the innovations term might not come
37
back down, even with or without a well modeled system. Therefore, a way of reducing
the Q matrix values is needed. One solution is to have a timer that when expired will
reduce the Q matrix values.
4.6 Other Kalman Filter implementations
In the following implementation, many different sensors are used for redundancy,
to be able to filter out sensor bias. The state matrix in this example grows to have 15
elements. The states contain the errors in the different sensors of the system. Before we
get to the filter, the dynamics have to be understood. We follow the same dynamics, but
the calculations are done a little differently. We first create a projection matrix (converts
from body coordinates to navigation coordinates). Then we project the accelerations
from the IMU into the navigation coordinate system. We then subtract the acceleration
due to gravity from the acceleration readings, and finally integrate along each axis in the
navigation coordinate system to determine the velocity and position of the vehicle. The
projection matrix C is shown below:
cos  cos 
cos  sin  
 sin   


C  sin   sin   cos   cos  sin   sin   sin   sin    cos  cos  cos  sin  
cos  sin   cos   sin   sin   cos  sin   sin    sin   cos  cos  cos 
When the projection matrix is used to describe a transformation from one iteration to the
next, it is propagated as follows:
38
C k 1  C k  Ak
Ak  I 3 x 3 
sin  
 0
x    z
  y


x  1  cos2   x2
z
0
x

y 

  x   dt
0 
  dt   x2   y2   z2
where dt is the sampling period and ω is the rotation rate around the axes.
This filter is somewhat more complex to the “position and velocity” example given
earlier. The filter dynamics are more complex because we have a dynamic system that is
time-variant and multi-dimensional. The filter thus has many states because noise in any
one sensor can cause errors that propagate through into states that are not measured by
that sensor (noise in the gyro-rate of the x-axis can cause errors in the final y-position for
example). The Kalman filter does a good job at using the system dynamics to filter these
errors out.
This filter is implemented using the formulas below for the terms we have seen earlier.
The matrices that make up these formulas will be explained in detail later.
System Dynamic Model:
x k   k 1 xk 1  Gwk 1
wk  N 0, Qk 
Measurement Model:
39
z k  H k xk  vk
v k  N 0, Rk 
State Estimate Extrapolation:
x k |k 1   k 1 x k 1|k 1
Error Covariance Extrapolation:
Pk|k 1  k 1 Pk 1|k 1kT1  GQk 1G T
State Estimate Observational Update:
x k |k  x k |k 1  K k z k  H k x k |k 1 
Error Covariance Update:
Pk |k  I  K k H k Pk |k 1
Kalman Gain Matrix:

K k  Pk|k 1 H kT H k Pk|k 1 H kT  Rk

1
This Kalman filter is used not to estimate the current position of the rocket, but rather the
errors in the measurements given by the sensors. Error dynamics in an inertial navigation
system are not linear, because errors in gyroscopic rates will influence errors in the
translation of the system (measured with accelerometers). If we assume the errors to be
small though, we can approximate the dynamics as linear.
Different implementations of Kalman filters have been done in the past. The number of
states has varied from 9 to 90 states (for 3D system – previous implementation shown
40
here had three 3-state filters and three 2-state filters). For this implementation, a set of
states which use only first-order errors is used. The state vector is:
 Error in North Position



Error in East Position


 Error in Down Position



 Error in North Velocity



Error in East Velocity


 Error in Down Velocity

 Error in North Attitude



x
Error in East Attitude

 Error in Down Attitude





Error in X Gyro Bias


Error in Y Gyro Bias




Error in Z Gyro Bias


 Error in X Accelerome ter Bias 
 Error in Y Accelerome ter Bias 


 Error in Z Accelerome ter Bias 
The state transition matrix is
  I 15x15  F  dt
where dt is the time between samples (sample period), and F is constructed based on the
following:

The error in position is the integral of the error in velocity

The error in the derivative of velocity is the sum of the cross product of the
specific force vector and the attitude error vector, and the projection of the
acceleration biases from body coordinates to navigation coordinates.
41

The error in the derivative of the attitude error vector is the projection of the gyro
biases from body coordinates to navigation coordinates.
The result of combining all these relationships gives us the F matrix. The fi are the
components of the specific force in the north ease and down directions. The ci are the
components of the rotation matrix that converts from body to navigation coordinates.
0
0

0

0
0

0
0

F  0
0

0

0
0

0
0

0
0 0 1 0 0
0
0
0
0
0
0
0
0
0 0 0 1 0
0 0 0 0 1
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
 fD
0
fN
fE
 fN
0
0
0
0
0
0
0
0
0
0
cNX
cEX
cDX
cNY
cEY
cDY
0 0 0 0 0
0
0 0 0 0 0 fD
0 0 0 0 0  fE
0 0 0 0 0
0
0
0
cNX
cNY
cNZ
0
0
0 0 0 0 0
0 0 0 0 0
0
0
0
0
0
0
cEX
cDX
cEY
cDY
cEZ
cDZ
0
0
0
0
0 0 0 0 0
0
0
0
0
0
0
0
0
0 0 0 0 0
0 0 0 0 0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0 0 0 0 0
0
0
0
0
0
0
0
0
0 0 0 0 0
0 0 0 0 0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0 
0 
0 

cNZ 
cEZ 

cDZ 
0 

0 
0 

0 

0 
0 

0 
0 

0 
The process noise is any noise that is present in the system, which is modeled as white
Gaussian noise coming from the sensors. Since the state variables are in navigation
coordinates, the noise sources from the accelerometers and gyroscopes of the IMU need
to be projected into navigation coordinates. This is done by multiplying by the C matrix.
Using this, we come up with the “noise model” G matrix shown below:
42
0 3 x 3
0
 3x3
Gw  0 3 x 3

0 3 x 3
0 3 x 3

wk  N 0,  k2
0 3 x3
C3 x3
0 3x3
0 3x3
0 3x3
0 3x3
0 3 x3
0 3 x3
C3 x3
0 3x3
0 3x3
0 3x3
0 3 x3
0 3x3
0 3x3
 0 3 x1 
w 
 XA 
0 3 x 3   wYA 


0 3 x 3   wZA 
0 3 x 3   w XG 


0 3 x 3   wYG 
0 3 x 3   wZG 


 0 3 x1 
0 
 3 x1 

The Q matrix is the covariance of the process noise in the system, and is obtained by
using the specifications (or real data) from the sensors that are used in the system, under
the same conditions for which the system is expected to be subject to.
We obtain the following for the Process Noise Matrix:
0
0

0

0
0

0
0

T
GQG  0
0

0

0
0

0
0

0
0 0
0
0
0
0
0
0
0 0
0 0
0
0
0
0
0
0
0
0
0
0
0
0
0 0  A2 0
0
0 0 0  A2 0
0 0 0
0  A2
0
0
0
0
0
0
0
0
0
 G2
0
0
0 0
0
0
0
0 0
0 0
0
0
0
0
0
0
0
0
0 0
0
0
0
0 0
0 0
0
0
0
0
0
0
0 0
0
0
0 0
0 0
0
0
0
0

2
G
0
0
 G2
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
43
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0

0 0 0 0 0 0
0 0 0 0 0 0

0 0 0 0 0 0
0 0 0 0 0 0

0 0 0 0 0 0
0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 0 0 0
0 0 0 0 0 0

0 0 0 0 0 0
0 0 0 0 0 0

0 0 0 0 0 0
Four redundant measurements of the rocket trajectory are used in this Kalman filter.
Using the compass, the error in yaw can be calculated. The altimeter can be used to
measure the error in vertical position. The velocity sensor can be used to calculate the
velocity error. Using these we arrive at the following:
Error in Yaw




Error in Zpositi on


z
 Error in Forward Velocity 


 Error in Sideways Velocity 
As discussed earlier, the measurement transformation matrix is used to transform the
measurement vectors into Kalman state vectors. When the state vector can be measured
directly, it is represented with a 1 in the H matrix. The velocity measurements are made
in body coordinates, so the north, east and down velocities have to be projected back to
body coordinates.
0
0
H 
0

0
0 0 0
0 1 0
0 0 cnx
0
0
cex
0
0
cdx
0 0 cny
cey
cdy
0 0 1 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0
Finally, in order to calculate the Kalman Gain matrix, we need to use the measurement
noise matrix R, which contains the covariance of the measurements.
44
2
 Compass
0
0 


2
R 0
 Altimeter
0 
2
 0

0
 Velocity


The problem with an implementation of a Kalman filter with many state variables is that
the calculations involved get exponentially more complex. For example, for this 15 state
filter, we must calculate a 15x15 matrix inversion, which is very computationally
intensive.
5
EXPERIMENTAL PROCEDURE
In order to properly analyze the DCM algorithm and choose a Kalman filter, different
experiments were conducted. Due to budget constraints, all experiments conducted used
a model of the MEMSense IMU, and the expected noise was modeled as zero mean white
Gaussian noise.
Therefore, this IMU model was used by all the different INS
implementations.
The first implementation that was carried out as part of this work was a series of
hardware blocks written in Verilog.
The first block was a conversion from the
MEMSense IMU two-byte two’s complement data format into 64-bit fixed-point, unit
meaningful data format (in G’s or degrees/second). The second block was an integrator,
which would output the iteration displacements, and the third block was the DCM
rotation algorithm.
The second implementation was an independent study that was carried out as part of the
requirements for the completion of the Masters degree. This independent study involved
45
developing a testbench to compare the performance of a microcontroller implementation
vs. a hardware block implementation of the INS. The microcontroller that was used was
the Texas Instruments MSP430. A performance analysis (speed, accuracy and resource
use) was carried out in order to properly asses the use of the MSP430 for this application.
As a tool to verify the correct implementation of the algorithms in the MSP430, a
MATLAB model of the INS was developed. Then, when designing the Kalman filter, it
was obvious that because of the many different implementations that were possible, the
flexibility that it has for merging sensor data and the difficulties that are always
encountered when tuning it, a MATLAB model of the filter was developed and added to
the MATLAB INS. Most of the re-usable work that will be of great use for future
students working on this project is in this MATLAB model of the IMU and INS system
(INS referring to both Kalman filter and DCM algorithm).
5.1 Implementations of the DCM algorithm
5.1.1
Number Representation
It is very important to decide upon a number format representation when mapping the
navigational algorithms to a hardware device. Using the ranges and accuracies needed
for all represented numbers in the algorithms, we can decide upon the best number
representation to use. The use of floating point format gives us much better accuracy at
the expense of extra computation. The use of fixed point format gives us less accuracy,
but we can use standard ALU blocks to perform operations. Many processors and
46
reconfigurable hardware computers do not have a floating point unit (FPU), which would
mean that floating point operations would have to be emulated.
A fixed point format representation was chosen, with a variable (user selectable before
compiling/synthesizing) point position and variable number of bits. The point position
and number of bits were made variable so that changes made to the navigational
algorithms can be easily accomplished if a change in point position is required.
Multiplying and addition functions were designed in order to be able to run the
algorithms in hardware using our selected number of bits and point position, to evaluate
different settings and make sure that we have enough bits and enough accuracy to
represent accurate but yet big enough numbers, and that these do not cause overflow
errors, inadvertently corrupting the algorithm results.
5.1.2
Generic Hardware Blocks
The integrator block that was used in the Verilog hardware implementation of the DCM
algorithm is shown below.
Figure 13.
Integrator Block
47
The function of this block was also coded in C and added to the MSP430 implementation
of the INS. Later, this functionality was also used in the MATLAB model of the INS
when the Kalman filter is not present. If the three-state Kalman filter is present, the
integration is done automatically.
The DCM algorithm block that was used in the Verilog hardware implementation of the
DCM algorithm is shown below.
Figure 14.
n-o-a-p Matrix Multiplier for DCM Algorithm
The function of this block was also coded in C and added to the MSP430 implementation
of the INS. Later, this functionality was also used in the MATLAB model of the INS.
As mentioned before, all the terms of the n-o-a-p matrix can be calculated using the
general formula shown below, which is what the MUL block from the figure above is
doing:
z  n  t1  t 2   o  t 3  t 4   t 5  t 6  t 7   a  t8  t 9   t10  t11  t12   p
48
5.1.3
Reconfigurable Hardware Implementation
At first, the top level for this implementation was a schematic in which the three main
blocks (conversion block, integrator block and DCM block) were connected together.
The first block was a conversion from the MEMSense IMU two-byte two’s complement
data format into 64-bit fixed-point, unit meaningful data format (in G’s or
degrees/second). The second block was an integrator, which would output the iteration
displacements, and the third block was the DCM rotation algorithm. Some multipliers,
adders, trigonometric lookup table blocks, barrel shifters and other smaller blocks made
up the three main blocks. After reviewing the design, the top level was written in
Verilog. Testbenches were written in order to test the functionality of all the separate
building blocks. No timing information was extracted from the work that was done.
5.1.4
MSP430 Implementation
As an independent study, some work was done to implement the DCM algorithm in a
Texas Instruments MSP430 microcontroller, and evaluate its performance.
This
independent study involved designing this embedded system using the MSP430x149 in
the MSP430-365 Development Board. The system actually consisted of the MSP430
Development Board and a model of an inertial measurement unit (IMU) running in
MATLAB. The application ran an algorithm that would receive data from the IMU and
calculate the current position of the IMU with respect to an initial position. Moreover,
the application was interrupt driven in order to receive data serially from the IMU and
also to manually reset the position. The purpose of the study was to gain insight into how
an inertial dead-reckoning system would perform implemented as a software application
49
in a low-power microcontroller, as opposed to a dedicated hardware block. What was
concluded from this independent study was that the MSP430 running with a 7MHz clock
was fast enough to accurately compute the position using the DCM algorithm and meet
the timing requirements of computing 100 iterations per second, but not fast enough to be
able to introduce a Kalman Filter.
5.2 MATLAB simulations
The first step that was taken was developing a model of the MEMSense IMU. This
model also included the expected noise modeled as zero mean white Gaussian noise.
After the model was finished, and a test trajectory (IMU values for an expected rocket
trajectory) was determined, the model for the INS was developed as a tool to verify the
correct implementation of the algorithms and the Kalman filter. The design of the
Kalman filter was much easier to test in MATLAB because of the many different
implementations that were possible, the flexibility that it has for merging sensor data and
the difficulties that are always encountered when tuning it. The MATLAB models of the
IMU and INS all use configurable parameters that can be easily changed in the future to
model different trajectories and noises change the settings on the Kalman filter. The INS
model assumes that there is no gravitational force, and thus an acceleration of zero in the
downward direction will cause no change in velocity in that direction.
5.2.1
Noiseless Trajectory
In order to create a reference “true-path” to evaluate the performance of all other
implementations, the INS was run with noise-less IMU data. The noiseless trajectory
was defined as an acceleration of 10 m/s/s in the upward direction for 80% of the total
50
flight time. After that, this acceleration drops to zero, and then there is only a constant
rotation totaling 95 degrees on one of the axes tangent to the surface of the Earth, thus
bringing the rocket into orbit. A plot of this trajectory is shown in the results section of
this paper.
5.2.2
Noisy Trajectory
Once the noiseless trajectory was defined, noise was added to the IMU data, and then fed
into the INS without the use of any filtering. This was done to be able to see how
effective the Kalman filter would actually be. The noise that was added was zero mean
white Gaussian noise with an SNR that is selectable by the user. For the simulations that
were performed, an SNR of 4 was used, which is very pessimistic for the IMU, but it
tested the capabilities of the AKF.
5.2.3
Single State Kalman Filter Trajectory
After noise was added to the IMU signals, the INS was run again using the single state
Kalman filter. The single state Kalman filter would only determine an estimate for the
accelerations and rotational rates, and then it would use the integrator described earlier to
come up with the iteration displacement and change in orientation, which was then fed
directly into the DCM algorithm.
This filter was not adaptive, and thus when the
trajectory hit the 80% mark (where the acceleration suddenly dropped), the model
became mismatched and the performance went down considerably.
51
5.2.4
Triple/Dual State Kalman Filter Trajectory
The INS was run again one last time using the triple/dual state Kalman filter. As we have
seen, this filter would determine an estimate for the accelerations, velocities and iteration
displacements in every axis, and also the rotational rates and change in rotation in every
axis. Therefore, integration was already achieved by the Kalman filter, and the values
from the state estimates could be directly fed into the DCM algorithm. This filter had a
switch that could make it adaptive, and thus when the trajectory hit the 80% mark (where
the acceleration suddenly dropped) and the model became mismatched, the system noise
matrix was quickly altered to match the system so the performance would not be affected
as much, as will be seen in the results section.
5.2.5
Kalman Filter Initialization
One of the problems in an INS is calculating the correct initial values for the states and
the covariance matrix. Any slight errors will propagate quickly, soon making outputs of
the navigation algorithm useless. Improper state error predictions have a much worse
consequence than a slightly higher overall state error. They lead to instabilities in the
system under many conditions. Since the initial states were known in the model, they
were used to initialize the Kalman filter. In the case that they are not known (like before
the rocket is launched), and update from the METEOR platform GPS could be uploaded
into the INS, giving it its position and velocity.
52
6
RESULTS AND DISCUSSION
The only truly meaningful results that were obtained from this study that can be used for
any analysis that could help future research in this project were obtained from the
MSP430 implementation and the MATLAB simulations.
Since the MSP430
implementation was performed as an independent study, a detailed description and
analysis is located in the appendix section of this paper.
As for the MATLAB
simulations, the results are discussed and analyzed in the following sections.
6.1 IMU Simulation
The IMU values that were generated for the 50 second simulations, with an iteration rate
of 100 Hz are shown below.
53
Figure 15.
Reference and Noisy IMU Values
As we can see, the blue lines indicate the real noise-less values, while the red lines
indicate the noisy values with white Gaussian noise. The noise is controlled by the SNR
which is user selectable. The SNR used for this simulation was 4.
6.2 Trajectories
The following figure describes the trajectories that were recorded for the 50 second
simulations, with an iteration rate of 100Hz.
54
Figure 16.
Detailed Rocket Trajectories for Various Implementations
The figure above contains trajectories for the true noise-less data and the noisy data with
no filtering, non-adaptive filtering, adaptive filtering and single state filtering. The three
dimensional plot is shown below with better detail.
55
Figure 17.
3-D Rocket Trajectories for Various Implementations
Also, the z-axis trajectory is shown below zoomed into the end of the trajectory, so we
can observe how the non adaptive filter starts to loose performance once the system
model starts being mismatched at the 40 second mark, when the z-acceleration drops and
the rocket starts turning. It must be noted that since the noise was modeled as zero mean
white Gaussian noise, due to the nature of the system, the final position of the noisy path
was very close to the final position of the true path. The advantage of using the filter is
that at any given time, the state estimate will contain more accurate information with a
lower variance up to that time.
56
Figure 18.
Z-Axis Displacement Towards the End of the Trajectory
6.3 Filter tuning and Innovations term
If we follow the innovations term (Y-HX) while the system goes through the change at
the 40 second mark, we see that it suddenly increases. The innovations term should have
a distribution that approximates to white noise, and have a relatively small variance.
When it suddenly increases like it does in the figure below, we know that the system is
being mismatched.
57
Figure 19.
Z-Axis Acceleration Kalman Filter Terms During a System Change
By using the innovations term, we can determine a multiple to use to increase the system
noise matrix Q. At this point, the Kalman gain K will change to start paying more
attention to the measurements, and less to the estimates. This means that at times that the
system is well modeled, the estimates will give us a better (and lower variance) result, but
when the system is mismatched, the measurements will give us a better result. From the
figure above we can also see that the variance of the estimates (almost zero) is much
lower than the variance of the measurements, and when the innovations term increases,
the variance of the measurements increases as well. The variance of the estimates also
increases, but its maximum value at 40.15 seconds is about 2E-3, which is quite low
compared to the variance of the measurements.
58
6.4 No filtering vs. Non-Adaptive Filtering vs. Adaptive Filtering
One of the most important discoveries that was made from this study is that in order for
the Kalman filter to properly and accurately work at all times is to have it adapt quickly
to system changes. It is for this reason that it was decided that at the 40 second mark, the
acceleration in the z-axis would undergo a sudden change. Notice that this change will
not affect the velocity other than it will stay constant, which would mean that simply the
rocket thrust has vanished, and it will keep moving at a constant velocity.
The figure below shows the z-acceleration values for the entire 50 seconds.
Figure 20.
Z-Axis Acceleration for Various Implementations During the Entire Trajectory
As we can see, the non-adaptive single stage Kalman filter has detrimental effects on
performance due to the system change. The non-adaptive triple/dual stage filter follows
59
the system change a little better, but the adaptive triple/dual stage filter follows the
change almost perfectly. In the figure below we can see a close up of the system before
the change.
Figure 21.
Z-Axis Acceleration for Various Implementations with an Invariant System
We see that the triple/dual stage filter follows the sinusoidal noise-less values with a
slight phase shift, but the variance of the estimates is quite low compared to the noisy
measurements. Much care needs to be used when tuning the Kalman filter, and we need
to have a good idea of the expected noise, because as we can see, if the Q-matrix values
are too small, in the case above the estimates would be a flat line, and the “real”
sinusoidal characteristics of the signal would be interpreted as noise (this could be though
60
of as a smoothing filter). In the other hand, if the Q-matrix values are too big, the
estimates will be almost the same as the actual noisy measurements, and thus there is no
advantage in using the Kalman filter. In the figure below we see a close up of the system
change at 40 seconds.
Figure 22.
Z-Axis Acceleration for Various Implementations During a System Change
Here, it is evident how when the system noise matrix values are not adjusted due to the
innovations term suddenly increasing (increase in variance), the estimates will take a
while to start representing data that is closer to the real data. As for the adaptive filter,
we can see how it quickly follows the sudden system change, and after the timer expires
(3 iterations), the estimates start to get smoothed out again.
61
7
FUTURE WORK
In this section we propose enhancements that could be done to the proposed INS, and
other work that is still necessary before implementing this INS on the METEOR rocket.
Some of the first steps that need to be taken in the continuation of this work are with
respect to the gravitational acceleration. In order to test the initial design of the DCM
algorithm and the Kalman filter, the gravitational acceleration was ignored. In order to
properly feed information into the DCM algorithm, we must first subtract the
gravitational acceleration from the “down” axis.
This acceleration will always be
present, and thus if the accelerometer on the down axis has a reading of zero, that means
that its velocity should be increasing in the upward direction. If the acceleration is –g, it
means that its vertical (orthogonal to Earth’s surface) velocity is not changing.
Another addition to the system could be adding the IMU’s magnetometer or a Doppler
velocity measurement or radar altimeter to the Kalman state vector, and better estimate
the current orientation of the platform. This way, long-term accuracy on orientation can
be achieved. It must be kept in mind that by adding another state to the state vector, the
matrix manipulations will become much more complex, and we must be sure that the
processor that is being used will be able to meet timing requirements with the extra
computation. Some research could also be done in trying to integrate the state vectors of
the accelerometers and the gyros, and perform the DCM algorithm in the state transition.
Although this would probably gives us a much better estimate of the current position
(current position would be part of the state vector), the computational requirements to do
this might be too complex for too little gain in performance; although this needs to be
researched more to draw any conclusions.
62
Looking forward past the design of the Kalman filter, a more realistic model of the IMU
trajectory needs to be defined, in order to “re-tune” the Kalman filter. Also, the control
system that will eventually use the current position and a “pre-determined” trajectory
needs to be designed, simulated and tested. The design of such a control system would
be greatly linked to the mechanical aspects of the rocket, specially the methods it uses to
steer. Much care needs to be put into the design of this system, and it must be kept in
mind that any changes in the mechanical aspects of the rocket, and any changes in the
control system would in turn affect the dynamic properties of the possible trajectory of
the rocket, therefore changing the system properties that are crucial for tuning the
Kalman filter. Thus, if such work is performed by two separate teams, they must be in
constant communication, and there should be some understanding, to a certain extent, of
the work being performed by the other team.
63
8
REFERENCES
[1] MEMSense products website: http://www.memsense.com
[2] “Introduction to Random Signals and Applied Kalman Filtering”, R.G. Brown and P.Y.C.
Hwang. Wiley, 1992.
[3] “Strapdown Inertial Navigation Technology – 2nd edition”, D.H. Titterton and J.L. Weston.
The institution of Electrical Engineers, 2004
[4] “A Generalized Acceleration Model for Kalman Filter Trackers”, D.F. Bizup. IEEE, 2004
[5] “Track Filtering of Boosing Targets”, G.J. Foster, J.J. Petruzzo and T.N. Phan. IEEE.
[6] “Evaluation of Solid-State Accelerometer for Positioning of Vehicle”, S. Nikbakht, M.
Mazlom and A. Khayatian. IEEE, 2005.
[7] “PHINS: The first high performances inertial navigation system based on fibre optic
syroscopes”, F. Napolitano, T. Gaiffe, Y. Cottreau and T. Loret. IEEE.
[8] “An extended Kalman Filter for Quaternion-Based Orientation Estimation using MARG
Sensors”, J.L. Marins, X.Yun, E.R.Bachmann, R.B. McGhee and M.J. Zyda. IEEE, 2001.
[9] “Strapdown Inertial Navigation System Algorithms Based on Dual Quaternions”, Y. Wu, X.
Hu, D. Hu, T. Li and J. Lian. IEEE transactions on aerospace and electronic systems, 2005.
[10] “An Introduction to Kalman Filters”, G.C. Dean. The Institute of Measurement and Control,
Institute publications, Volume 19, March 1986.
[11] “VLSI Implementation of Real-Time Kalman Filtering”, T.Y. Sung, Y.H. Hu. IEEE, 1986.
64
9
APPENDIX
9.1.1
MSP430 Implementation of the DCM algorithm
9.1.1.1 Introduction
The study compared and analyzed the performance and efficiencies of different design
solutions implemented with a microcontroller, and thus allowed them to be compared
with a dedicated hardware-block solution. The development of different designs was
geared towards complying with the functional specifications of the system.
This solution took into account the same physical constraints associated with the rocket,
and was a custom, design-specific implementation.
Since the objective of this
independent study was to simulate the operation of the hardware block in software, the
physical constraints were also taken into account for this study. The assumptions made
due to the constraints are stated in the functional specifications below and are analyzed in
the feasibility assessment, which assesses the different design solutions in order to meet
the specifications.
Overall, the system had to acquire the acceleration and rotation data from the IMU via a
serial link, and process the data in order to determine the IMU’s current position and
orientation with respect to its initial reference (at system start-up or manual reset). The
system had to be accurate enough to be able to reasonably determine a rocket’s position
after running for 1000 seconds. Since the actual rocket is currently still in its initial
design stages, the system was not tested on a rocket. Instead, the MATLAB model of the
IMU was used to generate “controlled trajectories”, which were then fed to the MSP430
65
and also to a MATLAB model of the dead-reckoning system. With these controlled
paths, we could re-simulate the paths continuously, and we could also feed the same
exact trajectory data to both the MSP430 and the MATLAB model for comparison.
In general, the requirements of this study were to design and implement an MCU-based
test bed that would enable the assessment of an IMU-based dead-reckoning system
performance in relation to incorporation of real-time filtering and software or hardware
based signal processing. Also, the test bed had to be capable of duplicating or emulating
capabilities of the current custom hardware based design. The test bed had to be capable
of being utilized in a moving vehicle and providing data necessary for performance
analysis. The test bed had to support development and execution of either a single
dedicated application or a set of tasks running under a real-time operating system.
The application was written in a modular fashion so that the code could be ported and
reused in the future, with only minor changes/configuration. The application prints the
resulting position and iteration number in the LCD, and can also send the information
through the second serial port. Also, while the application received and processed a
package from the IMU, the seven segment display would show the number of seconds the
application had been running.
9.1.1.2 MSP430 Development Board
In this implementation, SW18 and SW19 of the 365DB were used to generate interrupts
in the MSP430. SW18 and SW19 are connected to P1.6 and P1.7 on the MSP430. The
buttons are hardware de-bounced and active low; hence the interrupt must be setup for
falling edge triggering. The necessary registers in the MSP430 had to be setup to
66
generate interrupts on these switches. The registers that were configured were: P1DIR,
P1SEL, P1IE, P1IES and GIE bit in the SR (R2).
The two seven segment displays are connected to Port 2 [0-7]. Each segment and the
decimal point correspond to one pin on the MSP430. In order enable the two displays,
Port 3.0 and Port 3.1 need to be low. Since Port 2 controls both displays, in order to have
the displays “appear” to have different values, they need to be alternated with Port 3 and
Port 2 with the necessary values.
corresponding data.
Only one display is enabled at once, with the
To enable the seven segment displays, the registers that were
configured were: P2SEL, P2DIR, P2IE, P3SEL and P3DIR.
As for the LCD, functions were written to interact with the RAM controller in the LCD
module. These functions allowed the programmer to display a string on the 20x2 LCD.
Also, by sending different codes to the LCD via its db[0:7] pins (connected to P5 on the
MSP430), the LCD could be configured. In this configuration, the programmer could
shift all the current display information to the left or right on the LCD, or determine if the
cursor was active and if the current character was to blink, etc.
In order to be able to obtain timing information (to evaluate speed performance) from the
MSP430, general purpose pins were used. The pins were to be turned high before
starting a certain process and then low when the process finished, and with an
oscilloscope, we could determine how long the process took. Port 6 in the MSP430 was
used to gather timing information from the application.
67
9.1.1.3 Methods
The initial goals of this study were set by the requirements set above. Since the objective
of the study was to compare and analyze the performance and efficiencies of different
design solutions on the microcontroller, and also compare them with an existing
hardware-block solution, different performance measurements were taken and analyzed
to see if they satisfied the requirements for the METEOR Rocket. The development of
the different designs was geared towards complying with these functional specifications
of the system.
The initial specifications were:

The inertial dead-reckoning system will determine position and orientation from
data gathered from either a MemSense uIMU or from a PC running a model of an
IMU in MATLAB or another suitable program. The method of communication
between the system and the uIMU or program will be the same, no matter which
solution is used.

The system will acquire the acceleration and rotation data from the IMU or
program via an RS422 serial link configured as an 8-bit UART with one start bit,
eight data bits and one stop bit. One package containing all sensor data is 36
bytes long. The system should process the IMU data to convert it from signed 2’s
complement data into values that represent usable data (i.e. degrees/sec and m/s/s)
in order to determine the IMU’s current position and orientation with respect to its
initial reference (at system start-up or manual reset). The -3db bandwidth of the
analog sensors inside the IMU is 50Hz. The values are converted to digital values
inside the IMU.
68

Since testing on the rocket will not be possible, testing will be done in a car,
comparing position information with information from a GPS. The system will
keep track of position in meters, with a resolution of 1 meter, and keep track of
orientation in a “n-o-a” matrix (normal-orientation-approach) also known as a
direction cosine matrix. The accuracy in every axis of a ~2 kilometer trip in a car
should be ±100 meters of the actual position recorded by a handheld GPS (typical
accuracy is ±10 meters).

The system should be able to accurately determine a rocket’s position after
running for 1000 seconds. In order to achieve this, the system will have to be able
to perform one iteration of its calculations every 10ms (100Hz cycle). At an
iteration rate of 100Hz, the system will have enough data points to accurately
keep track of position, given the physical forces that it is intended to be subject to
(rocket => max 30G in thrust axis, ±1200deg/sec around thrust axis, ±45deg/sec
around thrust plane axes).

The application will reset its initial reference position with the press of a button.

Upon reset, the application will boot up, reset its initial reference position, and
start sending changes in position out through a serial port, at the rate of 1reading
every 5 seconds. This data can be captured by a terminal emulation program for
later analysis.

The system will also display the current position in meters (1 meter resolution,
1Hz refresh rate) with respect to the initial reference position on an LCD display,
to make the data visible to the user while the application is running.
69

The application will implement an optional Kalman filter to properly handle white
Gaussian noise present in the data gathered from the IMU. The filtering will be
done on 6 separate channels (1 for each axis for both acceleration and rotational
readings). Depending on the initial performance of a design without using the
filter, the filter can be turned on. In the event that an OS is used in the design, the
Kalman filtering will be implemented as separate tasks, so that the user can
choose to turn the filtering on and off, in order to compare the two designs.
In order to verify the functionality of the test bed and to evaluate the performance of the
system, a test plan was devised.
The test plan took into account the original
specifications of the system and compared them with the final performance. In order to
develop a test procedure, the program flow was first defined. The following is a list of
tasks that had to run in every iteration of the system:

Capture data from IMU

Convert data into known format (degrees/sec or m/s/s)

Run Kalman filter on data (optional) and obtain the integrated values w.r.t.
time(Δdegrees and Δposition)

Feed integrated values into matrix transform algorithm and obtain new n-o-a
matrix.

Extract Position values from n-o-a matrix and print them on LCD (every second)

Extract Position values from n-o-a matrix and send them out to the UART (every
iteration)
70
In order to evaluate this test bed (analyze the performance of a software implementation
vs. a hardware implementation of the design), the design was implemented and analyzed.
The system blocks were connected as shown in the figure below:
Figure 23.
System Block Connections
The next step was to do a feasibility assessment, in order to properly choose a
microcontroller that would satisfy the needs of the system. The system had memory
constraints and time constraints. As far as memory constraints, the necessary RAM for
storage of all the matrices, intermediate calculations and results was estimated. The
MSP430 met these requirements. As for speed requirements, the goal was to meet a
100Hz rate.
By estimating the amount of calculation that had to be done, it was
determined that the 7.3728MHz clock of the MSP430 Development Board would be
sufficient. The functionality of the blocks shown in figure 1 were implemented in the
71
MSP430. Figure 1 also shows the data flow of the system, starting on the IMU or the
Initial Values. The program flowchart is shown below:
Figure 24.
Program Flowchart
Once the system was functional, we were able to obtain information about the iteration
speed (speed performance) of the software design. Using the amount of time it took for
one iteration, we estimated how fast the design can run in software so we could compare
it to the hardware design.
72
The performance with respect to accuracy also had to be evaluated. This was done by
using the same set of data from the MATLAB IMU Model, and feeding the data to both
systems. Since the MATLAB IMU Model has the “noise-free” data available, the “true
trajectory” is known. The data that is fed to the software and hardware designs could
easily be altered with controlled Gaussian noise. After the data was fed to the designs,
and the outputs were recorded, they were compared to the “true trajectory”, and both
systems were evaluated.
9.1.1.4 First Approach – Floating Point Format
The first implementation of the algorithm used a floating point format in the MSP430.
The “double” data type was interpreted by the MSPGCC compiler as a 32-bit floating
point number. After this implementation was tested with a controlled trajectory, the
timing information from the oscilloscope capture was used to determine if it met the
specifications. Since there a large amount of mathematical calculations done in every
iteration, and for every calculation, the MSP430 would have to unpack the floating point
numbers, then operate on them, and then re-pack them again, each iteration took a longer
time than expected. For this reason, a second approach involving fixed point format data
types was pursued.
9.1.1.5 Second Approach – Fixed Point Format
The second implementation used 32-bit fixed point data types. Initially, the decimal
point was fixed to produce a (s 15 . 16) format (one sign bit, 15 integer bits, and 16
fractional bits). Later on, this was changed so that the programmer could easily change
73
the number of fractional bits by changing a #define statement in the code.
This
implementation with the fixed point format proved to be sufficiently accurate, and quite
faster than the floating point format implementation.
9.1.1.6 Results
Speed and accuracy performance results were gathered throughout the development of
the system. Using these results, the development process and the goals of the study kept
changing.
9.1.1.6.1 First Approach – Floating Point Format
Using the 32-bit floating point “double” data type, the Convert(), Integrate() and
MatMul() processes (one iteration calculation) would take on average 54 ms. In order to
achieve the 100Hz rate, these 3 processes must take less than 10ms. The reason these
processes took so long is because MatMul() contains a 4x4 matrix multiplication in
floating point format, where every single intermediate operation (be it addition or
multiplication) had to unpack, operate, and then repack the floating point numbers. Even
when the compiler was set to use different code optimization levels, the process took
around 45ms. There was also an accuracy issue, since the numbers would overflow after
a few iterations. Since the number format was to be changed to fixed point in order to
meet timing specifications, the accuracy issue was not addressed (later it was discovered
there was a bug in the MATLAB code that generated the IMU values).
9.1.1.6.2 Second Approach – Fixed Point Format
74
By using the (s 15 . 16) fixed point numbering format, the Convert(), Integrate() and
MatMul() processes (one iteration calculation) would take on average 2.15 ms. Then, by
setting the compiler to use the 16x16 hardware multiplier that is available on the
MSP430x149, each iteration would then take 1.4ms. At first there were some accuracy
issues, just like with the floating point format, but the issues were traced back to the
MATLAB IMU model having bugs. Also, once all the bugs were fixed, and having the
MSP430 calculate the sine and cosine of the angles in every iteration using the sin() and
cos() functions from the MSPGCC math library (which uses doubles, so a conversion to
doubles and then back to fixed point needs to be done), the iterations took 6ms. This
leaves us with about 4ms to complete the Kalman filtering, which will be left for future
work. The figure below shows an oscilloscope capture. The top channel is high when
the MSP430 is running the Convert(), Integrate() and MatMul() processes, and the
bottom channel is high when the MSP430 is receiving a package (interrupt driven, so it
can happen simultaneously – one package is 14 bytes).
Figure 25.
Oscilloscope Screen for One Iteration
75
With this new fixed point format, the multiply and add functions had to be written, since
two 32-bit fixed point numbers, with 16 bit fractional parts, multiplied together, result in
a 64-bit, shifted (by 16 bits) number. This 64-bit result then needs to be shifted and
casted down to the (s 15 . 16) format. The addition operation will produce a number with
2 sign bits, but granted the result does not overflow, we can just cast it down to (s 15 . 16)
again (no shift involved).
After fixing all the accuracy issues, the following results were obtained for two controlled
test trajectories that ran for 60 seconds at a rate of 100Hz:
MATLAB MODEL FINAL
MSP430 FINAL POSITION (m)
POSITION (m)
X acceleration = 1 m/s/s
1800
1798
Y acceleration = 0
0
0
Z acceleration = 0
0
0
MATLAB MODEL FINAL
MSP430 FINAL POSITION
POSITION (m)
(m)
X acceleration = 2 m/s/s
3600
3586
Y acceleration = 3 m/s/s
5400
5365
Z acceleration = 10*sin(2*pi*t/10)+10
18952
18924
Table 3: Position Results in MATLAB vs. MSP430
The include dependency tree is shown below:
76
Figure 26.
Include Dependency Graph for app.c
The next figure was created to better understand the program flow, and how all the
functions in the application are called.
77
Figure 27.
Call Graph for Function main()
78
Figure 28.
Call Graph for Function mat_mul_values()
We can see how many functions are called from more than one function, for example
LCD_PrintString. This call graph makes it easy to see where functions are called from,
so that if in the future this application is ever changed, a new programmer can easily
understand the program flow. Doxygen with this dot tool makes it very easy to navigate
through functions and dependency trees, and other call graphs to make it easy to
understand an application.
The resulting size of the text, data and bss sections (according to msp430-size from the
msp430 tools in cygwin) are shown below. As we can see, 21k is smaller than the
available 64k of memory on the MSP430x149, and the bss section is below the 2k of ram
available. We still have almost 1.5k of RAM available for the Kalman filter process.
79
Figure 29.
Size of Resulting Build from the ELF file
Although all the initial objectives of this study were not met, the results were satisfactory.
This system is a powerful tool that can be used in order to easily analyze the performance
of different number formats and algorithms used in a microcontroller solution, as
opposed to a dedicated hardware solution. If this software implementation using a lowpower microcontroller proves to be better than a dedicated hardware approach, the
system could perfectly be used in the actual METEOR rocket.
9.1.1.7 Future Work
With this study we developed a test bench in order to analyze a software implementation
of an existing dedicated hardware block. By having the system be configurable as far as
data transfer speeds, number representation formats, and having flexibility on what the
algorithm consists of, the system can be evaluated for different implementations.
The results of this study were satisfactory, although not all the initial objectives were met.
The specifications regarding timing and accuracy were met for trajectories without
rotation, and without using a Kalman filter.
80
This study was a good way to develop a test-bench for future work involving the
METEOR rocket guidance system. The student learned more about the components and
capabilities of the MSP430 development board, and how real-time data processing can be
quite challenging on a 16-bit low-power microcontroller in order to meet the rigorous
specifications of this time critical system. Also, it broadened the student’s perspective as
to how to program in a configurable and modular fashion and how this is important so
that the code is more readable and reusable for other configurations or even other
applications. This is important because when implementing the Kalman filter, or even
testing different algorithms and number formats, the programmer needs to be able to
easily change configurations to obtain different results. Also, it is very important to have
code that is easily adapted for use in different applications, and that is easily upgraded, or
redesigned by other people. By writing in a configurable and modular fashion, it is a lot
easier and time saving for another engineer to utilize the code. Also for an application
like this, it is even more crucial that the code be modular, since it is expected that the
code must be ported to other processors to compare performances.
Using what was learned in this study, future students that work on the METEOR rocket’s
inertial navigation system will be able to easily test their systems.
81