PAGE 1
LOW COST INERTIAL NAVIGATION: LEARNING TO INTEGRATE NOISE AND FIND YOUR WAY By KEVIN J. WALCHKO A THESIS PRESENTED TO THE GRADUATE SCHOOL OF THE UNIVERSITY OF FLOR IDA IN PARTIAL FULFILLMENT OF THE REQUIREMENTS FOR THE DEGREE OF MASTER OF SCIENCE UNIVERSITY OF FLORIDA 2002
PAGE 2
ii ACKNOWLEDGMENTS The research conducted in this thesis coul d not have been accomplished without the help of the Machine Intelligence Lab and th e Autonomous Submarine Team at the University of Florida who donated the use of th eir IMU. Their support was of great help. Special thanks go to Dr. David Novick a nd Dr. Paul Mason. Both of these individuals always provided me with a sounding board to bounce my ideas and concerns off of. They also provided me with an i ndispensable wealth of inform ation, knowledge, understanding, and friendship. For that alone I will always be grateful. Last, but not least, I would like to than k my loving wife, Nina Walchko, and our pack of wild cats. They suffered with me th rough the hard times a nd long hours, but were always there to help cheer me up when I needed it.
PAGE 3
iii TABLE OF CONTENTS page ACKNOWLEDGMENTS .................................................................................................ii LIST OF TABLES.............................................................................................................vi LIST OF FIGURES..........................................................................................................vii ABSTRACT....................................................................................................................... ..x 1INTRODUCTION ........................................................................................................1 Inertial Naviga tion ........................................................................................................1 Previous Work ...............................................................................................................2 Thesis Outlin e ...............................................................................................................4 2WHERE IN THE WORLD IS WALDO ......................................................................5 GPS Network Over view ...............................................................................................5 How GPS Works ...........................................................................................................6 NMEA Messages ....................................................................................................6 WGS-84 ......................... .........................................................................................7 Grids ........................................................................................................................7 Accuracy ...................................................................................................................... .8 Differential GPS ..........................................................................................................10 3INERTIAL NAVIGA TION ........................................................................................12 Overview of Inertial Na vigation Systems ...................................................................12 Gimballed INS ......................................................................................................12 Strap-down INS ....................................................................................................13 Reference Frames and Rotations ................................................................................13 Modelling the Ea rth ....................................................................................................16 Position on the Earth s Surface .............................................................................16 Gravity Model .......................................................................................................17 Navigation Equati ons ..................................................................................................17
PAGE 4
ivPosition and Velocity ............................................................................................17 Attitude .................................................................................................................19 Summary of Navigationa l Equations ..........................................................................22 4CORRECTING INERTIAL NAVIGATION .............................................................23 Sources of Er ror ..........................................................................................................23 Bias and Drift ........................................................................................................23 Temperature ..........................................................................................................24 Hysteresis ..............................................................................................................24 Vibrations ..............................................................................................................25 Extended Kalman Filter ....... .......................................................................................25 Position Error M odel ............................................................................................26 Attitude Error Model ............................................................................................27 Summary of Error Mode l Equations ...........................................................................29 5HARDWARE AND EXPERIMENTAL SETUP .......................................................31 Standard IMU Hard ware .............................................................................................31 Gyroscopes ............................................................................................................31 Ring Laser Gyro (RLG) ........................................................................................31 Fiber-Optic Gyros (FOG) .....................................................................................32 MEMS .........................................................................................................................3 2 Hardware Used ...........................................................................................................35 Crossbow IM U ......................................................................................................35 IMU Performan ce .................................................................................................36 Prefiltering IMU Data ...........................................................................................38 Garmin GPS ..........................................................................................................39 Experimental Setup .....................................................................................................40 6RESULTS ...................................................................................................................41 Navigational Soluti on Only ........................................................................................41 GPS/INS ......................................................................................................................4 2 7CONCLUSIONS ........................................................................................................49 AATTITUDE REPRESENTATIONS AND ROTATION MATRICES ......................50 Fixed Angle Rota tions ................................................................................................50 Euler Angles ...............................................................................................................51
PAGE 5
vQuaternions .................................................................................................................53 Quaternion Alge bra ...............................................................................................54 Rotations of Rigid Bodi es in Space. .....................................................................56 Attitude Errors in Quaternions ....................................................................................57 Summary of Quater nions ............................................................................................58 BKALMAN FILTERING AND ESTIMATION ..........................................................59 Introduction .................................................................................................................5 9 Kalman Filter Th eory ..................................................................................................60 Implementing The Kalm an Filter ...............................................................................62 Extended Kalman Filter Design ..................................................................................64 Augmented Kalman Filter ... .......................................................................................65 Estimation Models ......................................................................................................66 Controllability and Observabilit y of the Kalman Filter ..............................................66 Controllabilit y .......................................................................................................67 Observability .........................................................................................................67 REFERENCES .................................................................................................................68 BIOGRAPHICAL SKET CH ............................................................................................70
PAGE 6
vi LIST OF TABLES Table page 2-1. Accuracy of GPS 95% of the time. ........................................................................10 3-1. Coordinate system subscrip ts and superscript definitions. .....................................14 3-2. Earth model cons tants (WGS 84) ...........................................................................16 4-1. Positiona l errors ..................................................................................................... .23 5-1. Data conversions fo r the Crossbow IMU. ..............................................................35 5-2. IMU prefilter specifications. ..................................................................................38 6-1. Distances traveled as repor ted by the different systems. ........................................44 6-2. Distances traveled as repor ted by the different systems. ........................................46 A-1. Properties of a rotation matrix. ..............................................................................51 A-2. Comparison of the two ma jor types of rotations ...................................................52 A-3. Quaternion Al gebra Summary ...............................................................................54 B-1. Description of kalma n filter variables. ..................................................................63 B-2. Various models used in the kalman filter. .............................................................66
PAGE 7
vii LIST OF FIGURES Figure page 1-1. Boeing conversion kit which transforms a standard gravity bomb into a smart bomb. ...........................................................................................................................3 2-1. Orbits of G PS network. .............................................................................................5 2-2. Standard GPS sate llite in orbit. .................................................................................5 2-3. An airplane receiving a 3D position from 4 GPS satellites. ......................................8 2-4. How the UTM breaks up th e surface of the world. ...................................................8 2-5. WAAS coverage areas. Currently only the Pacific (pink) and Atlantic (yellow) satellites are up and running. ..........................................................................11 3-1. A flow chart of a strap-down INS whic h takes accelerations and rotation rates from the IMU and produces position, velocity, and attitude of the system. ............13 3-2. The XYZ frame is the inertial fr ame ECEF and the NWU frame is the local navigational frame, where the axes are north, west, and up. ..........................15 3-3. Body frame which is aligned with the axes of the IMU. The center of this frame is located at the origin of the navigational frame. ..............................................15 4-1. Overview of the extended kalman fi lters integration with the INS. .......................25 5-1. Ring laser gyro shown at top (note the triangular sh ape) and fiber-optic gyro diagram shown below. ..................................................................................................32 5-2. Spider mites walking on some MEMS parts. ..........................................................34 5-3. MEMS IMU. ............................................................................................................34 5-4. MEMS thru sters. ......................................................................................................34
PAGE 8
viii 5-5. Sensors used in the INS. (left) Th e Crossbow DMU-HDX which is a solid state vertical gyro capable of measuring angul ar rates and accelerations on all three axes. It also has the capability of measur ing the roll and pitch of the device too. (right) Garmin 16LVS OEM GPS which is both a receiver and antenna. ......34 5-6. Change in accelerometer reading ove r time due to temperature change. ................37 5-7. Random movement of syst em due to hyste resis. .....................................................37 5-8. This is a plot of the biases as the IMU was rotate d around the z-axis (yaw). Rotations around the other axes would also effect the biases, thus this mapping is not useful since the values ar e changing nonlinearly. ...........................................38 5-9. Comparison of the unf iltered data (top) produced by the IMU and the filtered data (bottom) using the Chebys hev II filte r. ...........................................................38 5-10. This is a test of the GPS accuracy. The GPS was set in a stationary location for 4 hours. The center of the plot was taken to be the average latitude and longitude reported by the sensor. Then the corres ponding distances from the average were calculated. This GPS receiver is capable of providing the standard 10 meter accuracy 95% of the time. ...............................................................................39 6-1. Map of the entire route taken from www.mapquest.com ........................................41 6-2. Number of satellites seen by the GPS receiver during the test. ...............................41 6-3. INS attitude solution with out extended kalman filter. ............................................42 6-4. INS results without GPS and kalman filter integrated into the system. ..................43 6-5. INS results with G PS and kalman filter integrat ed into the system. .......................43 6-6. This plot shows the interpolating capab ilities of the INS syst em in X and Y..........43 5-7. This plot shows the interpolating cap abilities of the INS system in Z. ...................43 5-8. Route taken for second test: starting at the commuter parking lot take North-South Drive, Archer Road, 34th Street , University, and back. .................................45 5-9. Results from the INS which matc h good with the map of the route. ......................45 5-10. Corner of Archer Ro ad and 34th Street. ................................................................45 5-11. The GPS data and INS solution for one of the stoplights on Archer Road. ..........46
PAGE 9
ix 5-12. The results from driving through a parking garage which blocks the GPS signal. The INS solution is shown on the left while the GPS readi ngs are shown on the right. The location of the parking garage and th e correct path are drawn on the plots. .........................................................................................................................47 5-13. Number of satellites s een during the expe riment. .................................................47 A-1. Body reference frame at tached to a rigid body. ......................................................51 B-1. Kalman filtering process at work, ta king noisy input measur ements and producing filtered output meas urements. .........................................................................64
PAGE 10
xAbstract of Thesis Presented to the Graduate School of the University of Florida in Partial Fulfillment of the Requirements for the Degree of Master of Science LOW COST INERTIAL NAVIGATION: LEARNING TO INTEGRATE NOISE AND FIND YOUR WAY By Kevin J Walchko August 2002 Chair: Dr. Mike Nechyba Major Department: Electrical and Computer Engineering Navigation is becoming more common in all areas of indus try and commercial sectors. The main tool being utilized is GPS. However there are situ ations in which higher levels of accuracy are require d which can not be achieved by GPS alone. This thesis will discuss the design and implementation of an inertial navigation system (I NS) using an inertial measurement unit (IMU) and G PS. The INS is capable of providing continuous estimates of a vehicles position and or ientation. Typically IMUs are very expensive sensors; however this INS will use a l ow cost version costing around $5,000. Unfortunately with low cost also comes low performance and is the main reason for the inclusion of GPS into the system. Thus the IMU will us e accelerometers and gyros to interpolate between the 1Hz GPS positions. All importa nt equations regarding navigati on are presented along with discussion. Results are presented to show the merit of the work and highlight various aspects of the INS.
PAGE 11
1CHAPTER 1 INTRODUCTION Navigation has been present for thousands of years in some form or another. The birds, the bees, and almost ev erything else in natu re must be able to navigate from one point in space to another. Fo r people, navigation had original ly included using the sun and stars. Over the years we have been able to develop better and more accurate sensors to compensate for our limited rang e of senses. This thesis will discuss work using one of these advanced sensors, an in ertial measuremen t unit (IMU). This sensor, coupled with the proper mathematical background, is capable of detecting acceler ations and angular velocities and then transforming those into the cu rrent position and orient ation of the system. Inertial Navigation Unfortunately Inertial Navigation Systems (I NS) have been relegated to the realm of military applications due to the extreme cost of the sy stems. High precision components must be incorporated into the IMU to produc e good results. The mo st critical are the gyros, which are used to determ ine the orientations of the sy stem. The orientation is critical to properly account for gravity. If gravity is accounted for in the accelerometer readings, then the system thinks it is moving when in fact it is not. Also high quality accelerometers are important. Id eally their should be no bias (or very low at least) and behave in a simple linear fashion. Unfortunate ly in real life, noise , disturbances, drifts, misalignments, and manufacturi ng complications enter into the equation and make developing an INS difficult.
PAGE 12
2If INS is so expensive, how do you create a low cost solu tion? The saving grace of low cost INS is the Global Positioning System (GPS) . This system provide s accuracies of 10 m 95% of time at an update rate of 1 Hz. The receiver only costs $100-$200 depending on size, features, etc. This syst em now provides a way to bound th e errors in an INS and the use of lower cost components in the IMU. Thus cheap but accurate INS systems can be developed for all sorts of applic ations that previously were cost prohibitive (i.e. consumer cars, remotely controlled vehicles, aut onomous vehicles, military munitions, etc). Previous Work INSs have been developed for a wide range of vehicles. Sukkarieh [1] developed a GPS/INS system for straddle car riers that load and unload ca rgo ships in harbors. When the carriers would move from ship to ship, they would periodically pass under obstructions that would obscure the GP S signal. Also, as the carriers got closer to the quay cranes, it became more difficult to get accurate pos itions due to the GPS signal being reflected about the cranes metal structure. This increa ses the time of flight of the GPS signal and results in jumps in the position. During thes e times the INS would then take over, and guide the slow moving carrier until a reliable GPS signal could be acquired. Mandapat [2] developed a low cost INS for Dr. Carl Crane here at University of Florida to replace their current system which is highly accurate, but very expensive. The current system is the Honeywell Modular Azimu th Positioning System (MAPS) and Ashtech Z-12 Differential GPS. This sy stem has an accuracy of less than 10 cm at a data rate of 10 Hz. The new system uses an IMU also developed from Honeywell which has ring laser gyros and costs about three ti mes the IMU used in this work. Mandapats GPS/INS produced positional errors within 1 to 2 meters of the MAPS system at a fraction of the cost.
PAGE 13
3Bennamoun et al. [3] develo ped a GPS/INS/SONAR system for an autonomous submarine. The SONAR added anot her measurement to help wi th accuracy, and provided a positional reference when the GPS antenna got submerged and could not receive a signal. Ohlmeyer et al. [4] develo ped a GPS/INS system for a new smart munitions, the EX171. Due to the high speed of the missile, update rates of 1 second from a GPS only solution were too slow, and could not provide the accuracy need ed. The GPS/INS smart munitions are cheaper than their mo re accurate cousins, Laser Guided Munitions (LGM). They are also immune to conditions which can greatly deteriorate th e accuracy of LGM, such as bad weather, heavy dust/snow storms, and fog. This integration of GPS/INS is a growing trend for military munitions (i .e. bombs, missiles, artillery sh ells, remotely operated vehicles). Boeing [5] has develope d a GPS/INS kit that converts old gravity bombs into precision-guided smart bombs. A cont rol unit is attached to th e end of the warhead which contains the GPS/INS system and battery powered motors to control the flight of the bomb. Actual use by American aircraft in Afghanistan dur ing the 2002 War on Terrorism proved these bombs can strike within 13 meters of their intended target. Figure 1-1. Boeing conversion kit which transf orms a standard gravity bomb into a smart bomb.
PAGE 14
4 Thesis Outline The remainder of this thesis will flow as follows. Chapter 2 will discuss GPS and familiarize the reader with how it works. Chapter 3 will in troduce all of the key concepts for inertial navigation and de rive all important equations. Chapter 4 will introduce problems inherent in inertial navigation a nd provide the necessary background for the extended kalman filter used whic h will aid in correcting thes e errors. Chapter 5 will provide background on hardware used in inertial navigation and the IMU and GPS that were used in this work. Chapter 6 will cover the experimental setup and present results using the developed INS. Various asp ects of the INSs performance w ill be presented to show its merit. Finally chapter 7 will present the conclusions drawn from this work.
PAGE 15
5CHAPTER 2 WHERE IN THE WORLD IS WALDO This chapter will give an overview of th e global positioning system (GPS). The purpose is to provide the reader with the basi c fundamental understanding of how GPS works and some of the technologies involved. Most of the inform ation in this chapter comes from three sources: Dr. Peter H. Dana [6], Garmin Users Ma nual [7], and Federal Aviation Administration (FAA) [8]. GPS Network Overview The GPS network of satellites contains a minimum of 21 sate llites that orbit the Earth at an altitude of about 11,00 0 nautical miles. They orbi t once every 11 hours and 58 minutes, so that they drift in the sky 4 minutes a day (they drift 2 minut es each orbit). There are 6 different orbits that the sa tellites 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. Figure 2-1. Orbits of GPS network. Figure 2-2. Standard GPS satellite in orbit.
PAGE 16
6 How GPS Works Finding your location using GPS is acco mplished by triangulating your position from the known positions of GPS sate llites. Distance meas ured from your position to the satellite is measured by time of fl ight of a signal sent from the GPS satellite. The signal sent from the satellite tells its lo cation and the time the signal was sent. Since the signal travels at the speed of light, the dist ance from the receiver to the satellite can be calculated. With a GPS receiver and one satellite, you can determine your possible location on a sphere with the satellite at the center. Unfortunately th is is not very useful. With two satellites we get two spheres that ar e centered at different locations. No w our position lies at the intersection of these two spheres, wh ich is a circle. Unfortunately this is still not useful. Now expanding the our network of sa tellites to three and a thir d sphere we get two possible locations the receiver can be in 3D space. Immediately one of th e two possible solutions can be discarded, leaving us w ith our position. Thus with th ree satellites, we can easily determine our positi on in 3D space. But this now rais es the question, how accurately can we calculate this position? This question will be answered in the next section which covers accuracy. NMEA Messages The signals or messages that are sent fro m the satellites are defined by the National Marine Electronics Associati on (NMEA). This group has defi ned standards for just about every possible device used for navigation a nd instrumentation. In fact the standards defined for use with GPS actua lly define not one, but many di fferent messages designed to provide every possible piece of useful information. NMEA al so allows hardware vendors to define their own proprietary messages . An example sentence might look like:
PAGE 17
7$GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*42 The current NMEA standard is 0183 versi on 2.0 which transmits data at 4800 baud. WGS-84 There are hundreds of datum that have been defined all over the world and through out the years. Some of these da tums are regional (such as the US datums NAD27 CONUS used by the USGS and NAD83 or the Europ ean 1979) and some are global (such as the WGS72). The World Geodetic Sy stem (WGS-84) is a world wide datum with its origin located at the center of the Ea rth and defines a refe rence ellipse. The reference ellipse is itself defined by the major a nd minor axes of the Earth, wh ich were determined by examining all known data in 1984. The data came fr om every possible sour ce including satellies. This is the datum which GPS uses to de termine altitude above the reference ellipse. Grids Now that we had defined a da tum for out coordinate system , we need to be able to divide the world up into nice pi eces. Most people are familiar with latitude and longitude which accomplish this division of the world. However the problem with this system is that Listing 2-1. Meaning of example NMEA message fields $ Start character of message GGA Global Po sitioning System Fix Data 123519 Fix taken at 12:35:19 UTC 4807.038,N Latitude 48 deg 07.038 N 01131.000,E Longitude 11 deg 31.000 E 1 Fix quality: 0 = invalid 1 = GPS fix 2 = DGPS fix 08 Number of satellites being tracked 0.9 Horizontal dilution of position 545.4,M Altitude, Meters, above mean sea level 46.9,M Height of geoid (mean sea level) above WGS84 ellipsoid (empty field) time in seconds since last DGPS update (empty field) DGPS station ID number *42 The checksum data, always begins with *
PAGE 18
8the world is round and not an nice even sphere. Navigation ha s defined one nautical mile as 1 minute of latitude along the equator. Si nce there are 60 minutes to each degree, there are 60 nautical miles per degree of latitude. The problem occurs as we increase our longitude, the distance between line s of latitude decrease. Thus at 45 degrees longitude there are only 42.426 nautical miles. GPS uses a more precise system calle d the Universal Tansverse Mercator (UTM). This divides the worl d up into 60 slices with minimal distortion compared to the traditional latitude/longitude system (an example of this is shown in Figure 2-2). Accuracy Time is a critical value in the equations that need to be solved to determine the receivers position in 3D spac e (to well under a micro-second) . Each satellite in the GPS network that orbits the Earth is equipped with an atomic cl ock so that they know exactly when they send a signal (which is encoded in the signal itself us ing a pseudo-random signal of 1023 bits). Unfortunately these clocks are very large and expe nsive. Thus it would Figure 2-1. An airplane receiving a 3D position from 4 GPS satellites. Figure 2-2. How the UTM breaks up the surface of the world.
PAGE 19
9not be feasible to embed these into every receiver on the market. So a compromise was made to include more inexpensive clocks into the receivers, but inst ead of treating time as a known in the GPS equations, it becomes a variable. Thus now a fourth satellite is required to reach a solution fo r the receivers 3D position. The assumption is also made that any error in the system comes from our clock being in error (w hich is an incorrect assumption). Thus the time variable can be changed to reduce the error in the solutions from the four satellites (Figure 2-1). Once the data is collected fr om the satellites, a set of 7 simultaneous equations and unknowns are solved. The unknowns are positions ( x, y, z), doppler (dx, dy, dz), and time. Proprietary methods are employe d when more that 4 satel lites are seen by the GPS to solve the now overdetermined solution. The equation from Pythagoras fo r solving a receivers position is [2-1] where X, Y, and Z are the posit ion of the receiver that we are trying to find, T is the time error at the receiver, Prs is the estimated range from the receiver to the satellite, and Xs, Ys, and Zs are the are the positions for each of the satellites. The Es te rm is the sum of all errors in the system, such as: clock errors, tr oposphere errors, ionosphere errors, etc. From this equation, only the receivers X, Y, Z pos ition and T are unknown. But with four satellites we get can get 4 equati ons. However in reality (as already mentioned above) the equations used have 7 unknowns which include the 4 mentione d here plus the doppler data dx, dy, and dz. Generally (since SA1 was turned off) most companie s will specify the accuracy of their GPS equipment as follows: PrsTEs ++ XXs ()2YYs ()2ZZs ()2++ =
PAGE 20
10 Differential GPS There are several kinds of DGP S available. The most comm on is the kind that receives corrections from a radio beac on. The beacon listens to transm issions which the U.S. Coast Guard sends containing correcti ons which provide better accu racy than stand alone GPS. This section will concentrate on a new t ype called Wide Area Augmentation System (WAAS) which is a product of the FAA to in crease the accuracy of GPS for aviation purposes. WAAS [9] utilizes ground stations which de tect and send GPS error information to a Master Control site. The Master Control site uses this info rmation to compute in order of importance or effect: 1.Integrity information 2.Ionospheric and Tropospheric delays 3.Short-term and long term satellite clock errors 4.Short-term satellite position error (Ephemeris) 5.Long term satellite position error (Almanac) This information is relayed to two WAAS geosynchronous Inmarsat satellites (AORW and POR) from the Master Cont rol Stations and is re-broadca st to user receivers as a 1.Selective Availability (SA) is a method used by the government to degrade the GPS signal globally so foreign powers and terrorists do not have precision positioning capabilities to attack the U.S or its allies. However on May 1, 2000, President Clinton ended the use of SA in an effort to increase its acceptance and use as a positioning system worldwide.Table 2-1: Accuracy of GPS 95% of the time. Method Accuracy GPS10 m DGPS (Coast Guard corrections) 1-5 m DGPS (WAAS)< 3
PAGE 21
11grid of corrections. From this grid, a GPS receiver interpolat es the proper Ionospheric correction based on its position in the grid. The extrapolation of this information outside the WAAS coverage is less and less precis e -to the point of INDUCING errors. Other errors are not location dependant. The WAAS correction information is differ ent than RTCM correc tions (transmitted by the Coast Guard for uses in DGPS) because WA AS decomposes the errors into their primary elements (Iono, clock, & ephemeris). RT CM, on the other hand, broadcasts pseudo range corrections which are the sum of all error sources as observed by the RTCM reference station. This information is only valid relatively close to the reference station. This is why spatial decorrelation is such a large fa ctor for RTCM, but not for WAAS (thus the reason it is wide area augmentation). Figure 2-1. WAAS coverage areas. Currently only the Pacific (pink) and Atla ntic (yellow) satellites are up and running.
PAGE 22
12CHAPTER 3 INERTIAL NAVIGATION This chapter will introduce strap-down iner tial navigation. Deriva tion for the navigational equations will be presented and discussed. Overview of Inertial Navigation Systems A basic flow chart of how inertial naviga tion works is shown in Figure 3-1. However, this is not all that needs to be done to have an INS that works. There are many problems with noise and unbounded error that must be handled to get any meaningful result out of the INS. Gimballed INS The first type of INS developed was a gimballed system. The accelerometers are mounted on a motorized gi mballed 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 platfo rm relative to the vehicle on which the INS is mounted. This setup has several detractors which make it undesirable. Bearings are not frictionless. Motors are not perfect (i.e. dead zones, etc.). Consumes power to keep the platform al igned with the naviga tional frame which is not always good on an embedded system. Cost is high due to the need for high quali ty motors, slip rings , bearings and other mechanical parts. Thus the typical customers for such sy stems were military uses on planes, ships, and intercont inental ballistic missiles.
PAGE 23
13Recalibration is difficult, and requires regular maintenance by certified personnel which could be difficult on an autonomous ve hicle. 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. Strap-down INS A strap-down system is a majo r hardware simplification of the old gimb alled 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 fr om the body frame to the navigational frame. This method overcomes the problems encount ered with the gimballed system, and most importantly reduces the size, cost, power c onsumption, and complexity of the system. Reference Frames and Rotations There are many different coordinate syst ems to choose from when doing navigation. Each has its own advantages and disadvantages so their use is really application specific. Earth Centered Inertial (ECI), also referred to as inertial for short. This system is fixed in inertial space with its or igin located at the center of the Earth. The Earth rotates around the z-axis and the x axis is aligned with the fixed stars.1 1.In reality the stars move, but their change in position relative to Earth is small enough to be neglected.Figure 3-1. A flow chart of a strap-down INS which take s accelerations and rotation rates from the IMU and pr oduces position, velocity, and attitude of the system.
PAGE 24
14Earth Centered Earth Fixed (ECEF) is similar to the inertial system except the axes rotate with the Earth. This system is al so used by the U.S. Defense Mapping Agency, which is good since the mil itary maps the world in a grid defined in meters not latitude and longitude. Since we will not be using la titude and longitude, this will be a better system to work with. Local Geodetic Vertical (LGV), this system is more convenient when used with latitude and longitude. This syst em proved to be more comple x, harder to understand, and more a computational system to work in though. Wander Azimuth Frame (WA) is used in high latitude regions where singularities hamper the previous systems. The previous systems always have an axis that faces North (referred to as North slaved), however the WA adds an a dditional variable alpha which allows the north facing axis to rotate . This is because magnetic north originates around Greenland, but the navigati on equations refer to true north which is located at the rotational axis of the Earth. Thus as airc raft or ballistic missi les pass over the North Pole, their compasses will point to Gr eenland while navigati onal north will point somewhere else. The angular difference between the two is alpha. Body reference frame is the c oordinate system associated with the vehicle. Typically, but not always, the x-axis point s out the front of the vehicl e, the y-axis is pointed out the right side, and the z-axis is pointed downward. This is the coordinate system in which all measurements will be taken. The reference frames which were used ar e shown in Figure 3-2 and Figure 3-3. Several rotation matrices are needed to transi tion between the various reference frames. A more detailed look at rotations is presented in Appendix A. The first rotation takes measurements in the body frame and puts them into the navigation frame, [3-1] Table 3-1: Coordinate system subs cripts and superscript definitions. Symbol Frame nnavigation b body cECI e ECEF Rb nc c s s c c s s s c s c + c s c c s s s + c s s s c s s c c c =
PAGE 25
15where is roll, is pitch, and is yaw. This rotation is the sequence 1-2-3, which is typically used in aerospace a pplications. This is a type 1 sequence which has singularities when the pitch is +/90 degr ees since at this angle both th e roll and yaw have similar effects. Thus for fighter airc raft which typically encounter this range, other methods must be included to account for this problem. The next rotation will transform points from the ECEF frame to the navigation frame, [3-2] where is latitude and is longitude. Now with these two rotations we can get another rotation, the one we really need. [3-3] Re ns c s s c s c 0 c c c s s = Rb eRn eRb n= Figure 3-2. The XYZ frame is the inertial frame ECEF and the NWU frame is the local navigational frame, where the axes are north (N), west (W), and up (U). Figure 3-3. Body frame which is aligned with the axes of the IMU. Th e center of this frame is located at the origin of the navigational frame.
PAGE 26
16 Modelling the Earth Periodically all satellite and geodetic data is used to determine the best fitting Earth model. The latest is the World Geodetic Syst em 1984 (WGS 84) and results of some of the important constants are shown below. Position on the Earths Surface Using the model, an extensiv e derivation (not shown here) can be done to determine a vehicles position on the Earth given its latitude, longitude , and altitude . The resulting equations are shown below. [3-4] [3-5] [3-6] [3-7] [3-8] Table 3-2: Earth model constants (WGS 84) Const Value R6,378,137.0 m 7.292115E-5 rads/s 9.7803267715 m/s^2 0.0193185138639 iegWGS 0gWGS 1RmeridianRe1 2 () 1 2 sin232 -----------------------------------= RnormalRe1 2 sin2 -----------------------------= xECEFRnormalH + () cos cos = yECEFRnormalH + () sin cos = zECEFRmeridiannpH + () sin =
PAGE 27
17 Gravity Model The Earths gravity field is not constant over its surface . One gravity model used to define the expected gravity seen at any point on the Earth is shown below. [3-9] [3-10] Navigation Equations Looking at Newtons sec ond law of motion, a change in motion occurs as a force is applied to a body. Now, divi ding both sides of the equatio n by the mass of the object results in the specific force. [3-11] In inertial navigation, acceler ometers detect accelerations due to forces exerted on the body. These forces are typically re ferred to as specific forces (S). Thus readings from the IMU will be referred to as specific fo rces, which are independent of the mass. Position and Velocity Using the Coriolis theorem, the equations wi ll be derived in the ECI (inertial) reference frame. Remember the ECI frame has its z-ax is aligned with the ax is of rotation of the Earth and is fixed in inertial space so it does not move. The Coriolis theorem states that the total velocity of a vehicle () in a non-rotating reference frame is equal to its ground speed () plus the a dded speed due to the rotation of the Earth. Here the subscript i refers to the non-rotating inertial frame, ECI. ggWGS 01 gWGS 1 sin2+ () 1 2 sin2 ()12 /----------------------------------------= gSHC c g g g = f m --aS == vivs
PAGE 28
18[3-12] where the rotation of the Earth is and r is the vehicles position. Now differentiating this with respect to the inertial reference frame. [3-13] The rotation of the earth is constant (), therefor e its derivative is zero (). Now substituting for the velocity from the original equation we get. [3-14] The acceleration term on the left hand side of the equation can be replaced with all of the accelerations in the system. [3-15] Where is the specific force (i.e. all of th e accelerations seen by the IMU) and is the gravity model of the system. Fi nally solving for the acceleration of the vehicle results in the navigation equations which when integrat ed will turn IMU accelerations into velocities. [3-16] where the term subtracted from the specific force is the coriolis acceleration and the term subtracted from the gravity is the centripetal accelerati on. Although these terms are small, as we will see, small consta nt acceleration offsets produce a velocity ramp when integrated. This in turn produces a quadratic position change when the velocity ramp is integrated. vivsieri× + = ie00 ie T= v ·iv ·s ·ieriiev ×i+ × + = ieconst = ·ie0 = v ·iv ·sievsieier ×i[] × + × + = Sigi+ v ·s · ievsieier ×i[] × + × + = Sigiv ·sSiievsgiieier ×i[] × + × =
PAGE 29
19Now these equations, which where derived in the inertial frame, must be transformed into the ECEF (Earth) reference fram e. Luckily this is an easy task. [3-17] Thus the inertial equation becomes: [3-18] These equations can also be represented in a state space form. [3-19] where P is the position and V is the velocity vector in the ECEF co ordinate system. Also the cross products in the previous equati ons are replaced with a skew matrix. [3-20] Attitude The attitude of the IMU is need so that the measuremen t taken (which are in the body frame) can be rotated in to the Earth frame. To accomplish th is, quaternions will be used to represent the attitude of the IMU (and hence the vehicle itself sin ce the IMU is bolted on to the vehicle). But why use quaterni ons and what the hell is a quaternion1 any ways? Well lets answer the first part of that question. Quater nions have the following good properties (in no particul ar order) for use in navigational systems. 1.See Appendix A for a more detailed explanation of quaternions.v ·ev ·sievs× = v ·sSe2 ievegeieier ×e[] × + × = V ·eP ·e2 ie e ie eie e I 0 V P Rc eRb e00 gSHC cSb+ = ie e0 ie0 ie00 000 =
PAGE 30
20They are computationally efficient. Subse quent rotations (for example a rotation about the x-axis followed by the rotation about the y-axis) is accompl ished by multiplying a 4x4 by a 4x1. While in Euler space, two subs equent rotations would be handled by multiplying a 3x3 by 3x3 matrix. Quaternions require less memory to repr esent them. Euler angles require nine elements to represent a rotatio n (3x3 matrix) while a quatern ion only requires four elements. However this is a weak point, but some embedde d applications do have very constrained memory requirements. The propagation of a quaternion in time always results in an orthogonal rotation matrix. An IMU will supply angular rates fr om its gyros which wi ll contain noise. Ideally if the IMU is rotated 90 degrees in a plane, the integr ation of the data from the gyros will properly reflect th is 90 degree rotation. However, since there is noise, the IMU will report rotations on al l axes. The real problem is that the resulting 3x3 rotation matrix is not guaranteed to be orthogona l any more which mean s that the inverse of the rotation matrix is not equal to the transpose. Now extra steps must be taken at each time step to make the rotation matrix orthogonal again. To answer the second part of the question, a quaternion is a way to represent the orientation of a rigid body, but also it is a way to represent a rota tion about an arbitrary axis in space. A quaternion can be thought of as a complex number which contains a real and imaginary part. However, the imaginary part of a quaternion contains three values while the real part only has one. The attitude of an object is assumed to be represented by a quaternion in which the imaginar y part is the elements 1-3 a nd the real part is the fourth element. [3-21] [3-22] where is a unit vector corresponding to the ax is of rotation and is the angle of rotation. q q13 q4 T= q13 q1q2q3 Tn 2 -sin == q4 2 -cos = n
PAGE 31
21For navigation we will need to be able to convert from ro ll, pitch, and ya w into quaternion space and back again. Th is is easily done by the following set of equations. [3-23] Alternatively a rotation matrix can be calc ulated directly from the elements of a quaternion. This is useful since there are no sine or cosine functions call ed during this process. Trigonometric functions te nd to be very computationall y expensive and in systems that require high performance are replaced with lookup tables or other faster (less accurate) implementations. Once the attitude is put into quaternions, the kinematic di fferential equa tions for attitude rotation are given as follows. [3-24] [3-25] This equation allows re adings from the gyros to be us ed to update the current quaternion, which represents the attitude of the system. q c 2 --c 2 -s 2 -s 2 --s 2 -c 2 - c 2 --s 2 -c 2 -s 2 --c 2 -s 2 -+ c 2 --c 2 -c 2 -s 2 --s 2 -s 2 - s 2 --c 2 -c 2 -c 2 --s 2 -s 2 -+ = tan sin tan 2 q1q22 q4q3+ 2 q4 22 q1 21 + ---------------------------------2 q4q22 q1q3 2 q2q32 q4q1+ 2 q4 22 q3 21 + ---------------------------------= q · 1 2 -q = 0 z yxz0 xyy x0 z x yz0 =
PAGE 32
22 Summary of Navigational Equations The navigation equations for the Earth Cent ered Earth Fixed (ECE F) system are combined below in state space form. [3-26] [3-27] where is the rotation rate of the earth, R is a rotation matrix between different coordinate systems, P is the position and V is the velocity vector in the ECEF coordinate system as denoted by the superscript e. V ·eP ·e · 2 ie e ie eie e0 I 00 00 Q V P Rb eRb e00 00 gSHC cSb+ = ie e0 ie0 ie00 000 = Q 1 2 -0 z yx z0 xyy x0 z x y z0 = ie
PAGE 33
23CHAPTER 4 CORRECTING INERTIAL NAVIGATION Low cost inertial navigati on unfortunately is not a si mple task. There are numerous sources of errors, noise, and dist urbances that must be overcom e. This chapter will discuss some of these problems and outline a solution. The solution will come in the form of the extended kalman filter. The necessary e quations will be derived and discussed. Sources of Error This section will provide a quick overview of some difficulties pr esent in inertial navigation, and provide a better understanding for the difficulties enc ountered with the IMU. 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. [4-1] Table 4-1: Positional error that results from biases after a time of 100 seconds and 30 mins. Bias (m/s^2) Error (m) t=100 sec. Error (m) t = 30 mins .1500162000 .01 5 16200 .001.51620 .0001 .05 162 error 1 2 -biast2 =
PAGE 34
24Looking at the Table 4-1 it beco mes apparent that determini ng the bias is of critical importance if any accurate measurement is expected. The drift rate has a similar, and an equally massive impact on the position of a system. If a drift is not properly accounted for, and th e IMU thinks it is rotating, then the navigation equations will not properly account for gravity and the syst em will think it is moving due to a maximum accelerati on of 9.8 depending on how far the sy stem has drifted. Temperature The IMUs accelerometers and gyros are sensitive to te mperature as shown by Nebot and Durran-Whyte [10]. Thus as the temperatur e of the IMU changes, the associated bias and drift will change until th e temperature reaches steady st ate or remains the same. This is not critical in our applicat ion, we just wait for the IMU to reach steady state before trusting the readings. However if this system wa s mounted in an aircra ft which changed altitude and temperatures, this would be a problem. Hysteresis The drift rates and acceleromet er biases tend to change e ach time the unit is switched on. This is due to the fact th at measurements are noisy. Typi cally 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 b ecause not everything has infi nite bandwidth). When random noise is filtered, this produces what is calle d a random walk. The integration of this random walk will result in velo city and positions moving at di fferent rates during different runs even though the IMU (and vehicle) are in the same or ientation and experiencing the same accelerations during each run. ms2
PAGE 35
25To give an idea of the performance of a strap-down system, the following quote is taken from an article [11] written by A. D. King, Chief E ngineer of Navigation and Electro-optic Systems Division of Marconi Electronic Systems. Marconi produces INS for virtually all of the RAFs combat aircra ft as well as many other systems. Many of these instrument errors vary each time you switch the system on INS have good days and bad days. To characterize th e performance of an INS, you have to resort to statistics, and take the r.m.s. total error from an ensemble of many representative missions. A ty pical standard expected from a good INS produces an error that increases with time (not an en tirely linear fashion), and reaches .6 miles after one hour (referred to as .6 nautical miles/hour system). 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 resona nce 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. Extended Kalman Filter An extended kalman filter was developed to estimate the biases and drifts of the system and then update the navi gational solution. The full kalm an filter equations are presented in the Appendix B, but an overview of the process is shown in Figure 4-1 and further information can be found in Brown and Hwang [12]. Figure 4-1. Overview of the extended kalm an filters integration with the INS.
PAGE 36
26Kalman filtering relies on a dynamic model of th e system. The following error models for both position and attitude were developed based on derivations by Chatfield [13] and Rogers [14]. Position Error Model The position of an object in th e inertial frame rotated in to the earth frame is given below. [4-2] Taking the derivative of this equation and replacing the de rivative of a rotation matrix with its alternative form results in: [4-3] Again, taking the derivative and some more algebra. [4-4] Finally the equations of motion for navigati on are found. This deriva tion resulted in the same answer as previously given. This form is assumed to represent both the computed and real dynamics of navigation in the earth frame. [4-5] Now defining the actual value () as the computed value () minus some error () and substituting this into the above equation () where we are assuming the equation represents the actual values. [4-6] PeRi ePi= P ·iie iRe iPeRe iP ·e+ = P ··eRe iP ··e2 ie eP ·eie eie ePe++ () = P ··e2 ie eP ·eie eie ePe++ geSe+ = P P P PP P = P ··e2 ie eP ·eie eie ePe++ () P ··e2 ie e P ·eie eie e Pe++ () Se Seg g +++ =
PAGE 37
27Now canceling the computed values on each side and assuming the and the error in gravity is also negligible, we have a way to estimate error in the system. [4-7] The last remaining term on the right hand side of the equation still has to be defined. Using the idea that the error in the measured accelerations is due to misalignment of the system, we can rewrite the right hand side (w here the terms with an underline represent skew matrices). [4-8] [4-9] However this derivation made use of th e following definition by Chatfield [13]: [4-10] Attitude Error Model Similar to how the position er ror was derived, the attitude error model starts off by looking at a rotation from the body frame to the earth frame. Then the definition for computed values is substituted in to the equation and simplified. [4-11] [4-12] [4-13] where the product of error is zero and the term cancels itself out. Now postmultiplying both sides with and renaming the terms. [4-14] ie eie e0 P ··e2 ie e P ·eie eie e Pe++ Se= Se Rb eSbeRb eSb == eSe See== Rb eRb eRb e eRb e == Rb eRn eRb n= Rb e Rb e Rn e Rb n () Rb n Rb n () = Rb e Rn e Rb n Rn eRb n = Rb eRn eRb n= Re b Ee Nee+ =
PAGE 38
28where [4-15] [4-16] [4-17] The term is a skew-symmetric form of th e attitude error, is the skew-symmetric form of the error in the orient ation of the instrument cluste r, and the is the skew-symmetric error of the angular e quivalent of the position error. Now that we have the nifty defi nitions of attitude error, we need to find the error in the earth frame from our calculat ions of euler angles. Using our definition of the computed rotation matrix we have [4-18] Now pulling out true rotation from the right hand side, substituting in our definition of error, and simplifying. [4-19] Next, take the derivati ve and solve for error. [4-20] [4-21] [4-22] Substituting in equation [4-19] and some more algebra. [4-23] Ee Rb eRe b = eRn e Rb nRe b = Rn e Rb nRn bRe n Rn enRe n == Ne Rn eRe n = Eee NeRb eRb e Rb e = Rb eI Rb eRe b () Rb eI + Ee() Rb e== R ·b e E ·eRb eI + Ee() R ·b e+ = eRb e E ·eRb eI + Ee()eRb e = E ·eeRb eRe b I + Ee()e+ = E ·eeI + Ee() I + Ee()e+ =
PAGE 39
29[4-24] Now comes the magic, Chatfield then claims the above equation can be reduced to the following where the skew error matrices are transformed in to error vectors. [4-25] The instrument attitude erro r vector deferential equation is derived in a similar way. [4-26] Summary of Error Model Equations The complete model is presented below. This filter model is small compared to other models which have anywhere between 20 and 50 different states, depending on how their navigational models were defined. Note that there is also th e inclusion of tw o sets of terms which now makes this an extende d kalman filter model. The te rms are the errors in bias on the accelerometers, and drift of the gyros. E ach is modelled as a random walk (or could have modelled them as a Mar kov process), where the terms wi th the subscript N on the far right of the equation are zero mean, random white noise with the appropriate standard deviation. The purpose of this is to estimate these new parameters, since they are difficult to determine, and (as in the case of the bias) change greatly depending on temperature, time, and orientation. [4-27] E ·ee Ee Eeee++ = e ·ee eeb e ·eeeb e V · P · · S ·b ·bA V P SbbB SN bN b+ =
PAGE 40
30 [4-28] A 2 ie e ieieSeRb e03 x 3I3 x 303 x 303 x 303 x 303 x 303 x 303 x 3b e03 x 3R b e06 x 15= B 06 x 6I6 x 6= Se0 Sz e Sy eSz e0 S x eSy e Sx e0 =
PAGE 41
31CHAPTER 5 HARDWARE AND EXPERIMENTAL SETUP This chapter will provide an overview of ha rdware typically used in inertial navigation. Also the two primary sensors used in this work, the IMU and GPS, will be discussed. Standard IMU Hardware The two sensors that make up an IMU are the gyros and ac celerometers. The gyros are the most important since they will be used to determine the orienta tion of the system. This is crucial since this allows the INS to properly account for gravity and other known biases, in which even small errors can qui ckly accumulate in the system. Gyroscopes Gyroscopes, or more commonly gyros, are sens ors that measure the rotation rate of a system. This is important so the measurem ents taken by the IMU can be properly orientated from body coordina tes to some navigational referenc e frame. There are many different types of gyros present on the market. Ring Laser Gyro (RLG) The RLG shown in Figure 5-1 is very expe nsive, but accurate si ngle degree of freedom sensor for rate determination designed to replace mechanical gyros . It consists of a laser, a closed pathway (two way) in a triangular shape, mi rrors at each corner, and an interferometer/photodetector. The frequency with which la ser pulses travel around the pathway is constant when the gyro is sitting still. However, as the gyro is rotated the frequency increases or decreases depending on if the p hotodetector has rota ted closer or far-
PAGE 42
32ther from the emitted laser pulse. Thus rotati on rate can be determin ed from the frequency change of the laser. A laser be am is typically sent in both di rections of the configuration. Fiber-Optic Gyros (FOG) The FOG shown in Figure 5-1 is a lower cost alternative to RLG, where the path is a coil of fiber-optics and there is a beam splitter located at the beginning and end of the coil. There is also a laser source a nd a detector so that the fre quency of the laser light can be determined. MEMS Micro-Electro-Mechanical Sy stems (MEMS) is the integration of mechanical elements, sensors, actuators, a nd electronics on a common silico n substrate through the utiliFigure 5-1. Ring laser gyro shown at top ( note the triangular shape) and fiber-optic gyro diagram shown below.
PAGE 43
33zation of microfabrication te chnology. While the electron ics are fabricated using integrated circuit (IC) process sequences (e.g., CMOS, Bipolar, or BICMOS processes), the micromechanical components are fabricated using compat ible micromachining processes that selectively etch away parts of the silicon wafer or add new structural layers to form the mechanical and el ectromechanical devices. MEMS promises to revolutionize nearly every product category by bringing toge ther silicon-based microelectronics with micromachining technology, thereby, making possible the realizati on of complete systems-on-a-chip. MEMS is truly an enabling technology allowing the development of smart products by augmenting the computational ability of microelectronics with the perception and control capabilitie s of microsensors and micro actuators. MEMS is also an extremely diverse and fertile te chnology, both in the applications, as well as in how the devices are designed and manuf actured. MEMS technology make s possible the integration of microelectronics with act ive perception and control func tions, thereby, greatly expanding the design and application space. Some pictures of MEMS devi ces are shown in Fi gure 5-2 to give an idea of the size. Here these microscopic creatures appear to be giants compared to the miniature mechanical gears they walk over. Another device is shown in Figure 5-3. This one is a 6 DOF IMU made by Integrated Micro Instruments (which was bought out by An alog Devices Inc.). The device measures only 5 mm by 9 mm and is complete with three gyros and three accelerometers. MEMS is not limited to ju st sensors but also propulsi on. A MEMS thruster made by TRW Space and Electronics is shown in Figure 5-4 which coul d be used in space applica-
PAGE 44
34tions. This device is capable of delivering 10E-4 Nsec of impulse from the poppy seed sized cells which contain the le ad styphnate fuel propellant. Figure 5-5. Sensors used in the INS. (lef t) The Crossbow DMU-HDX which is a solid state vertical gyro capable of measuring angular rates and accelerations on all three axes. It also has the capability of measuring the roll and pitch of the device too. (right) Garmin 16LVS OEM GPS which is both a receiver and antenna. Figure 5-2. Spider mites wa lking on some MEMS parts. Figure 5-3. MEMS IMU.Fi gure 5-4. MEMS thrusters.
PAGE 45
35 Hardware Used This section will cover the tw o main pieces of hardware us ed in this thesis, the IMU and the GPS shown in Figure 5-5. An attempt wi ll be made to evaluate their performance and show their shortcomings when used individually. Crossbow IMU The IMU is a solid state vertical gyr o (DMU_HDX) from Crossbow Technologies intended for airborne applicati ons such as UAV control, Avi onics, and Platform Stabilization. This high reliability, st rap-down inertial subsystem provides attitude measurement with static and dynamic accuracy comparable to traditional spinning mass vertical gyros. Data will be transmitted by the DMU digitally via a serial connection (RS-232) and converted to the proper format using the conver sions in Table 5-1. The gyros on the Crossbow IMU are low cost, low performance MEMS (Mechanical Electrical Micro-Systems) gyros. These gyros are much le ss expensive to produce, but pe rformed at least an order of magnitude worse than anothe r low cost IMU system bein g developed by Rommel Mandapat [2]. That system uses an IMU developed from Honeyw ell which has ring laser gyros. Unfortunately, the gyro performance is a critic al element in accounting for gravity in the system. Table 5-1: Data conversions for the Crossbow IMU. Data Equation X, Y, and Z Acceleration Roll and Pitch AccelG () dataGR 1.5 215------= Angle ° () data 180 215-------=
PAGE 46
36 IMU Performance Lets look at the performance of the IMU a little closer. As disc ussed earlier, temperature plays a role in the accur acy of the IMU. The first test , shown in Figure 5-6, shows the change in the accelerometer readings over a period of 2.5 hours whil e the IMU was sitting still on a table. Since th e IMU is capable of recording its temperature, we also have a temperature history during this test. The IMUs accelerations do change slightly (remember that a offset of only .001 over a period of 30 minutes will result in a position error of 1620 m) but the exact amount of change is difficul t to see due to the ex cessive amount of noise in the IMU readings. Thus the data will have to filtered prior to using it. All of the little offsets on the accelerometers, noise, te mperature effects, etc will amount to positional error, but how much? Again, taking four se ts of data while the IMU was sitting still on a table and subtracting off the mean values of each set, which are assumed to be the biases. Each data set c overed approximately 35 minutes and data was taken at 10 Hz from the x ax is accelerometer. Then integr ating the resulting data should result in zero positional cha nge since it is not moving and biases were taken care of by subtracting of the mean valu es (assuming that the noise s een in the system is random white noise with a zero or c onstant mean). However as show n in Figure 5-7 this is shown to not be true. The IMU does think it has m oved because the noise, temperature effects,Angular Rates Internal Temperature Table 5-1: Data conversions for the Crossbow IMU. Data Equation Rate ° s () dataRR 1.5 215------= Temperature ° C () data 5 4.096 ------------1.375 44.44 =
PAGE 47
37biases, etc (all or some) must be changing with time to pr oduce these offsets. The system thinks that it has move -6410 m, -1054 m, 343 m, or 4673 m de pending on which set of Figure 5-6. Change in accelerom eter reading over time due to temperature change. Figure 5-7. Random move ment of system due to hysteresis.
PAGE 48
38data you look at. Thus not only do the biases change with orientati on and temperature, but also it depends on when you take the data. Prefiltering IMU Data The data produced by the IM U is extremely noisy, thus a filter was designed. Matlabs signal toolbox was used to acco mplish this task. The toolbox is capable of designing all of the classic FIR and IIR filte rs. A IIR filter was decide d on since it produces the same results as an FIR filter but wi th a much lower order. This lo wer order results in a less computational process. The following specific ations for the filter were decided on: Table 5-2: IMU prefilter specifications. Specification Value Pass Band2 Hz Stop Band 3 Hz Stop Band Attenuation-50 dB Figure 5-8. This is a plot of the biases as the IMU was rotated around the z-axis (yaw). Rotations around the ot her axes would also effect the biases, t hus this mapping is not useful since the valu es are changing nonlinearly. Figure 5-9. Comparison of the unfiltered data (top) produced by the IMU and the filtered data (bottom) using the Chebyshev II filter.
PAGE 49
39Additionally, the desired filte r should not have any ripple in the pass band range, thus the Equiripple, Elliptic, and Ch ebyshev I filters were elimin ated as possible designs. The remaining Butterworth and Cheyshev II filt ers were looked at. After much testing with various options, the Chebyshev II filter was se ttled one as the best one for the job and its performance can be seen in Figure 5-9. Garmin GPS The GPS system used in this work is the Garmin 16LVS. Garmin is a common name in commercial civilian GPS systems, and th is OEM device has performance that is on par with all other GPS systems av ailable currently (i.e. accuracy of about 10 m 95% of the time) as shown in Figure 5-10. However th is GPS was specific ally bought because it included a WAAS (Wide Area Augmentation Sy stem) filter which should increase the accuracy to less than 3 m 95% of the time. Figure 5-10. This is a test of the GPS accuracy. The GPS was set in a stationary location for 4 hours. The center of the pl ot was taken to be the average latitude and longitude reported by the sensor. Then the corresponding distances from the average were calcula ted. This GPS receiver is capable of providing the standard 10 meter accuracy 95% of the time.
PAGE 50
40 Experimental Setup The two primary pieces of hardware used for this these have been covered, and now the rest of the setup will be explained. The IMU and GPS were connected to a 200 MHz laptop running Linux1 by serial2 connections. The IMU and GPS were powered from a battery pack. The INS was mounted in a car (with the GPS magnetica lly mounted to the outside roof) and driven on local streets. The navigational software was a multi-thre aded program with five main threads. Threads are a more modern wa y to do multi-proce ssing using a standard called pthreads or POSIX Threads. The main thread displayed va rious information to the screen during the experiment using menu system written in ncur ses. The second thread talked to the IMU over the serial connection. It al so did the prefiltering of the data and then inserted the information into shared memory. A third thread talked to the GPS and inserted that information into shared memory. Another thread performed the navigation using the information that was being inserted into shared memo ry. All of the navigational equations and the kalman filter were implemented in C. Several libraries had to be written to perform matrix and vector operations along with numerical integration of di fferential equations and kalman filtering. The final thread was a data logger which wrot e selected information from shared memory to a data file for later analysis and plotting in Matlab. 1.Linux is a free unix clone operating system written by Linux Torvalds. 2.Laptops only come equipped with a single serial port, thus the second serial port needed was a USB to serial converter.
PAGE 51
41CHAPTER 6 RESULTS The experiment is broken up in to two parts. The first part is the navigation solution which does not utilize the kalm an filter or the GPS positional corrections. The second part will include these so the limi tation of the IMU and benefits of the kalman filter and GPS can be seen. Navigational Solution Only The first set of results was wit hout the use of the extended kalm an filter, to see if it was really necessary. The INS was driven along the route shown in Figure 6-1. During the test (although not used in this phase of the test) the GPS was able to see plenty of satellites to generate good 3D positions. The results of esti mating the roll, pitch, and yaw without any corrections is shown in Figure 63. The estimated angl es appear to track the true angles to Figure 6-1. Map of the entire route taken from www.mapquest.com Figure 6-2. Number of satellites seen by the GPS receiver during the test.
PAGE 52
42an acceptable degree. The IMU is capable of reporting its true roll a nd pitch, but not yaw. Assuming the performan ce between estimating the yaw, p itch, and roll angles are the same, it should not be necessary to require a compass to upda te the true yaw angle. The couple degrees of error should not effect INS results much since the car is traveling on flat roads. The performance changes when we look at Figure 6-4. The GPS and INS (i.e. using the navigation equations and IM U data only) differ greatly. Thus the GPS with the kalman filter must be included into the INS to give any good results. GPS/INS After the inclusion of the GPS and kalman fi lter, the plot shown in Figure 6-5 is much better. The GPS and INS lie right on top of each other. Taking a closer look at this plot, Figure 6-6 and Figure 67 show that the two do not really lie exactly on top, but rather the INS transitions smoothly through the GPS points. Figure 6-3. INS attitude soluti on with out extended kalman filt er. The estimated roll, pitch, and yaw are shown by the solid line, while the tr ue roll and pitch reported by the IMU is the dashed line.
PAGE 53
43Looking specifically at Figure 6-6, it can be seen that the IMU is picking up some of the accelerations in the turn and shifted the position left of the GPS points. But going into the turn and once the turn is completed, the INS and GPS positions merge back together. Figure 6-7 is a better exampl e showing how the INS is able to take the discrete GPS position and the accelerations from the IMU a nd fit a curve through the two. This level of continuous positio ning can not be offered by GPS alone. Figure 6-4. INS result s without GPS and kalman filter integrated into the system. Figure 6-5. INS result s with GPS and kalman filter integrated into the system. Figure 6-6. This plot shows the interpolating capabilities of the INS system in X and Y. Figure 6-7. This plot shows the interpolating capabilities of the INS system in Z.
PAGE 54
44Finally the distances traveled during the e xperiment were calculated and the results were close as shown in Table 6-1. The cars odometer was felt to be the most accurate and the GPS and INS distances are on either side of the value. The extended kalman filter atte mpts to estimate the biases and drifts present in the system to increase the accuracy of the system . However there appeared to be no difference between using the estimated biases and drifts from the filter or using constant ones. This is attributed to the excessive amount of noise fr om the low cost IMU. Chatfields [13] work was the prime motivator for in cluding these terms in the extended kalman filter, but he assumed measurements that were much better (i.e. less noisy) than the ones being produced by the Crossbow IMU. Thus this part of the kalman f ilter could be eliminated to reduce computational expense w ith no loss of performance. Another test was conducted. The route is shown in Figure 68 and the result from the INS are shown in Figure 6-9. There is a good corresponden ce between the INS solution and the route taken. Again the influence of the accel erometers can be seen in th e data for the intersection of Archer Road and 34th Street, s hown in Figure 6-10. Another in teresting result seen in this plot, is the path just prior to the second stoplight. After the turn was made onto 34th Street, the vehicle switched from the right lane, to the center lane , and back to the right lane again. These motions were picked up nicely by the INS. Table 6-1: Distances traveled as reported by the different systems. GPS INS Car km4.895.16 miles 3.04 3.21 3.1
PAGE 55
45Several stops were made duri ng the trip due to stoplights on the route. The results are shown in Figure 6-11 for one of the stops. Ideally the INS solution would not move since the vehicle is stopped and there would be no accelerations. The GPS positions would Figure 6-10. Corner of Archer Road and 34th Street. Stoplight Changing lanesFigure 6-8. Route taken for second test: starting at the commuter parking lot take North-South Drive, Archer Road, 34th Street, University, and back. Figure 6-9. Results from the INS which match good with the map of the route. Intersection
PAGE 56
46move due to the amount of error associated wi th GPS. The more sens ors that can be incorporated into the system, the better the results would be. If the velocity of the vehicle, from the speedometer could be incorporated into th e system, this would help the INS to reject the moving GPS data and the noisy IMU reading and should show that the vehicle is standing still. The distances reported by the three systems are shown below. In this case, the GPS distance were closer to the distance measured by the odometer . This is accounted for by the fact that the road surface was rougher (i.e. more bumps in the road) which effected the IMUs accelerometers. Table 6-2: Distances traveled as reported by the different systems. GPS INS Car km9.239.46 miles 5.73 5.87 5.61 Figure 6-11. The GPS data a nd INS solution for one of the stoplights on Archer Road.
PAGE 57
47The final test looked at the effects of loos ing the GPS signal for a short period of time. The INS was first driven in a parking lot and th en into a parking garage. Inside the garage, Figure 6-12. The results from driving through a parking garage which blocks the GP S signal. The INS solution is shown on the left while the GP S readings are shown on the right. The location of the parki ng garage and the correct path are drawn on the plots. Figure 6-13. Number of satell ites seen duri ng the experiment. Parking Garage
PAGE 58
48the GPS signal was eventually lost. The resu lts are shown in Figur e 6-12. After entering the parking garage, the GPS eventually loos es the satellite signa l (shown in Figure 6-13) and is unable to determine its position. Again, the inclusion of other sensors to aid the INS (in addition to a better IMU) w ould have produced better results.
PAGE 59
49CHAPTER 7 CONCLUSIONS This thesis has shown the effective combin ation of two different sensors: GPS and IMU. Each sensor al one has its own strengths and weak nesses. The low cost IMU used in this work is not capable of running by itself and pr oviding any reasona ble positioning information. GPS alone provides good results, but is only capable of determining position every second with 10-15 meters of error 95% of the time. The two sensors combined have the capability of producing good estimates of position in between the one second updates and overcoming their individual weaknesses. Together they are capable of showing the small scale movements (i.e. ve hicle changing lanes) that ar e not apparent with GPS alone due to accuracy and the 1 Hz position rate.
PAGE 60
50APPENDIX A ATTITUDE REPRESENTATION S AND ROTATION MATRICES When dealing with a subject such as iner tial navigation, contro lling spacecraft, or computer vision, it is common to have to deal with multiple reference frames and have to relate the position of one point in one frame to its pos ition in another reference frame. Thus it is important to know how to rotate points from one frame to another efficiently and understand what limitations ma y be present in various methods. This Appendix will attempt to familiarize the reader with quatern ions, rotations, attitude in space, and euler angles. Comparisons will be drawn between euler angles a nd quaternions to show their strengths and weaknesses. Fixed Angle Rotations Typically when trying to rela te a common point between di fferent reference frames, a rotation matrix will be constr ucted. This matrix will allow easy transformation between frames. [A-1] Here a point (P) is rotated fr om where it is originally de fined, in reference frame a, to another reference frame b. Not that the rotati on matrix R does not include any translation, but only rotation. Note also that R has the following properties: PbRa bPa=
PAGE 61
51When a rigid body is rota ted about a fixed inertial referenc e frame, it is referred to as a fixed angle rotation. These types of rotations are common in robotic s where one is trying to determine the joint angles of a serial robot relative to some point in space. What is important to understand, is rotati ons of this type are made a bout the inertial axis which is inconvenient. A better method would be rotati ons about the body axes. These type of rotations are known as Euler Angles. Euler Angles Euler angles are typically though of in terms of roll, pitch, and yaw angle about body axes. These terms are shown graphically below for a rigid body in space. Table A-1: Properties of a rotation matrix. Rotation Matrix R R1 RT= Ra bxayazaxbybzb== R 1 = columniR () rowiR () 1 == Ra cRb cRa b= Figure A-1. Body reference fr ame attached to a rigid body. The x-axis points out the front of the vehicle.
PAGE 62
52[A-2] There are two different groups of euler rota tions out of the possible twelve. The two groups are distinguished by th eir singularity locations. Typi cally aerospace and navigational applications use the 1-23 rotation where this singularit y only effects fighter aircraft who dive or climb at such steep angles. Some space applications, such as orbits around the Earth, use the 3-1-3 rotation sequence. In order to perform these ope rations on the rigid body, a rota tion matrix (R) is used to find the new orientation of th e spacecraft given th e old orientation. Given below are the attitude matrices for rotations about the x, y, and z-axes. [A-3] [A-4] [A-5] Table A-2: Comparison of the two major types of rotations, sequential and first and third axes rotation Singularities in Pitch Possible Rotation Sequence Type I+/-901-2-3, 1-3-2, 21-3, 2-3-1, 3-1-2, 3-2-1 Type II 0/180 1-2-1, 1-3-1, 2-1-2, 2-3-2, 3-1-3, 3-2-3 roll pitch yaw = Rx () 100 0 () cos () sin 0 () sin () cos = Ry () () cos0 () sin 010 () sin0 () cos = Rz () () cos () sin0 () sin () cos0 001 =
PAGE 63
53Now, subsequent rotations a bout these primary axes can be accomplished by multiplying the matrices together. Thus, successive rotations about the z-axis, x-axis, and y-axis are given by: [A-6] Quaternions Quaternions where invented by William Rowan Hamilton in 1843. Prior to his discovery, it was believed impossible that any alge bra could violate the laws of commutativity for multiplication. His work introduced th e idea of hyper-complex numbers. Here real numbers can be thought of as hyper-complex numbers with a rank of 1, ordinary complex numbers with a rank of 2, a nd quaternions with a rank of 4. Hamiltons crucial rule that made this possible: [A-7] Hamilton supposedly developed this rule whil e on his way to a part y. When he realized what the solution was, he took out his pocket knife and carved the answer into a wooden bridge. This rule woul d forever change mathematics as was known at the time. Now mathematicians could look at algebra where communitivity did not work. This is where Gibbs and others developed algebra of vector spaces, and quickly eclipsed Hamiltons work until recently. Quaternions, also known as Euler symmetri c parameters, are mo re mathematically efficient ways to compute ro tations of rigid and non-rigid body systems than traditional methods involving standard rota tional matrices or Euler a ngles. Quaternions have the advantage of few trigonometric functions needed to compute attitude. Also, there exists a product rule for successive rotations that greatly simplifies the math, thus reducing procesRzyx ,, () Rz () Ry () Rx () = i2j2k2ijk 1 ====
PAGE 64
54sor computation time. Quaternions also hold th e advantage of being able to interpolate between two quaternions (throu gh a technique called spherical linear interpolation or slerp) without the da nger of singularities, maintaining a constant veloc ity, and minimum distance travelled between points. The major disadvantage of quaternions is the lack of intuitive physical meaning. Most people would understand where a point was if they were gi ven [1 2 3]. However, few would comprehend where a point was if given the quaternion [1 2 3 4]. This section does not attempt to provi de the extensive understanding needed to employ quaternions but rath er a simple introduction. Quaternion Algebra The quaternion is composed of a scalar and a vector part. The scal ar is a redundant element that prevents singularit ies from occurring since the f our elements are all dependent upon each other.1[A-8] where the v stands for vector and the r is a scalar real. Given belo w is a table that will summarize the important mathema tical operations of quaternions. 1.The order of the quaternion el ements is not standardized. I ha ve chosen the element order that NASA uses which is the imaginary part first then the real. Some authors put the real first then the imaginary which is similar to complex numbers. Thus before you use any equations that utilize quaternions, make sure to understand how the author is arranging the elements.Table A-3: Quaternion Algebra Summary Operation Formula Add / Subtract Scalar Multiplication q qxqyqzqr Tq1q2q3q4 Tvr , ixjykzr +++ ==== qq ' ± qxqx' ± qyqy' ± qzqz' ± qrqr' ±T= q qx qy qz qr T=
PAGE 65
55Multiplication is an important operation for quaternions, t hus we will elaborate on it a little. [A-9] where [A-10] [A-11] [A-12] [A-13] or rewritten in matrix form. [A-14]Norm Quaternion Multiplication aConjugate Inverse a.Notice that the order of the quaternions in the matrix multiplication have been reversed.Table A-3: Quaternion Algebra Summary Operation Formula qqx 2qy 2qz 2qr 2+++ = q ' q q4q3q 2q1q 3q4q1q2q2q 1q4q3q 1q 2q 3q4q ' = q q xq yq zqr T= q1 q q ------= p qrr4ir1jr2kr3+++ == r4p4q4p1q1 p2q2 p3q3 = r1p4q1p1q4p2q3p3q2 ++ = r2p4q2p1q3 p2q4q3q1++ = r3p4q3p1q2p2q1 p3q4++ = p q r1r2r3r4p4p3 p2p1p3p4p 1p2p 2p1p4p3p 1p 2p 3p4q1q2q3q4q4q3q 2q1q 3q4q1q2q2q 1q4q3q 1q 2q 3q4p1p2p3p4===
PAGE 66
56The last part is the same representation present in the table of quaternion operations above. Notice how only the signs change when we reverse the order of the multiplication in the matrix-vector form of the quaternion multiplication. This is because of the cross product relationship with the vector part: . Rotations of Rigid Bodies in Space. Quaternions are able to repr esent a unique rotation in sp ace. To perform a rotation () of a rigid body about an arbitrar y moving/fixed axis (e) in sp ace, the quaternion representation of this operation is: [A-15] [A-16] Notice that only one sine and one cosine f unction call is needed to calculate a quaternion, where euler would require three sine and three cosine function calls, one each for roll, pitch, and yaw. Since tr igonometric function calls are co mputationally expensive, this is a great savings. Rotation of a point in space with a quaternion is done as follows: [A-17] [A-18] Since the quaternion has four elements, the point P must ha ve a zero appende d on to it in place of the real part of the quaternion. Thus the point P values constitute the imaginary (vector) part of the quaternion. This rotation can also be done similar to euler and fixed qp × pq × = q13 e 2 -sin = q4 2 -cos = e e1e2e3 T= PbqPaq = Paxyz 0T=
PAGE 67
57angle rotations by creating a rotation matrix fr om the quaternions. The attitude matrix (A) or rotation matrix (R) for this is [A-19] Successive rotations can be accomplished by multiplying the attitude matrices together. Hopefully it can be seen th at quaternions are better th an other Euler operations to determine attitude. Quaternions lack singularities due to one redundant elemen t. They also lack the computationally inte nsive trigonometric functions, a nd contain a simplified way to determine successive rotati ons about an arbitrary axis. Attitude Errors in Quaternions Since quaternions can represen t attitudes and not just rota tions, it is important to be able to calculate the differen ce between where you want to point and where you are pointing. For example, say you had to control where a satellite was to point and you need an error term to feed in to your control system. [A-20] where is the quaternion error, is the attitude of the satel lite, and is the target attitude. Note that the quaternions are in reve rsed order in the matrix and the spacecraft quaternion has the imaginary part negated due to the inversion. Aq () R q1 2q2 2q3 2 q4 2+2 q1q2q3q4+ () 2 q1q3q2 q4() 2 q1q2q3q4 () q1 2 q2 2q3 2 q4 2++2 q2q3q1q4+ () 2 q1q3q2q4+ () 2 q2q3q1q4 () q 1 2q2 2 q3 2q4 2++ == qEqS 1 qTqT 4qT 3q T 2qT 1q T 3qT 4qT 1qT 2qT 2q T 1qT 4qT 3q T 1q T 2q T 3qT 4q S 1q S 2q S 3pS 4== qEqSqT
PAGE 68
58 Summary of Quaternions Quaternions have singularities at angles of rotation of 180 degrees , but this is generally not a problem. Typically if an application encounter s a rotation greater than 180 degrees, it is shorter to go in the other direction. The quaternion is a compact method of re presenting a unique ro tation with only four elements with out the redundant information present in euler rotations nine elements. Quaternions are difficult to visualize wit hout extracting out the axis of rotation and rotation angle. Although the quaternion can be thought of as a sphere in 4D space, this does the majority of people little good, sinc e they can not perceive a 4D surface. The use of quaternions allows you to com pute the shortest distance between two different attitudes. If lerp is used, the resul ting velocity between the two attitudes will not be constant but more bell shaped. If slerp is used, the resulting velocity will be constant. This process is difficult using eule r angles, which contain many singularities that must be avoided and the solution is not guarant eed to be the shortest distance. Also the solutions that result from eu ler angles are not uniformly distributed along the surface of a sphere. Thus it again is not always possible to tran sition from one orientation to another using the shortest di stance, because these empty areas must be skirted around to arrive at the desired location.
PAGE 69
59APPENDIX B KALMAN FILTERING AND ESTIMATION This Appendix will introduce the reader to kalman filtering. First some background material will be give n, then a simple explanation of the fundamental concepts. Further explanation about kalman filtering can be obtained in Brown and Hwang [12]. Introduction The kalman filter is an alternative way to calculate the minimum mean-squared error (MMSE) using state space. R. E. Kalman, a graduate research professor in the electrical engineering department of Univ ersity of Florida, first de veloped the filter in 1960. Some of the advantages that the kalm an filter has over other estima tors are: computational efficient by recursively processing noisy data, real-time estimator, can be adapted to non-stationary signals, handle complicated time-var iable multiple-input/output systems, vector model random processes under consideration. The kalman filter estimates a process by using a form of feedback control. The filter estimates the process state at some point in time and then obt ains feedback in the form of noisy measurements. The filter equations fall into two gro ups: time update equations and measurement update equations. The time update equations ar e responsible for projecting forward (in time) the current st ate and error covariance estimate s to obtain the a priori estimates for the next time step. The measuremen t update equations are responsible for incorporating a new measurement into the a priori estimate to obtain an improved a posteriori
PAGE 70
60estimate. The time update equations can also be thought of as predictor equations, while the measurement update equations can be thought of as corrector equations. Kalman Filter Theory This section will give a brie f derivation of the kalman filt er which follows the derivation in Brown and Hwang [12]. The kalman filter assumes th e random process to be estimated is of the form1: [B-1] where x is a state value, u is a control effort, w is white noise with known covariance. When measurements are taken of the process at discrete moments in time, they occur according to the fo llowing relationship: [B-2] where z is a noisy sample, D is the direct transmission of th e input to the output, C is the ideal (noiseless) connection be tween the measurement and the state, and v is measurement error. This process can be modelled discretely in the following form, assuming there are not control inputs u to the system. [B-3] [B-4] The system error is defined as: [B-5] 1.Typically the state space system is represen ted as , and this fo rm will be used when discussing the controller. However, during the di scussion of the kalman filter the above notation () will be used since this form is more common in kalman filter literature, and will be used otherwise. There is no di fference between the representations other than notation.x · AxBu + = x · FxBu + = x · AxBu + =x · FxBuGw ++ = zHxDuv ++ = xk 1 +kxkwk+ = zkHkxkvk+ = ek xkx k =
PAGE 71
61where is the best estimate prior to re ceiving a measurement at time . The error covariance matrix at this time is: [B-6] where E[*] is the expected value. Now a li near blending of both the estimate and a measured value is taken. [B-7] where is the new updated estima te, z is the measured value, and K is a weighting value that determines the amount of error between the measured value and the best estimate. This gain is referred to as the kalman gain which is capable of changing value over time. Now looking at the error covariance of this new updated estimate, we get the following. [B-8] [B-9] Now, after some algebra and realizing that is the estimation error which is uncorrelated with the measurement error v, th e following expression results for the error covariance. [B-10] This is a general expression for updating the er ror covariance matrix , and it applies for any value of K. What is needed is a K that will minimize the i ndividual terms along the main diagonal of P si nce these represent the estimation error variances for each state that is being estimated. x k tkPk Eek ek T [] Exkx k () xkx k ()T[] == x kx k KkzkHkx k () + = x kPkEekek T[] Exkx k () xkx k ()T[] == PkExkx k () KkHxkvkHkx k + () [] xkx k () KkHxkvkHkx k + () () []T[] = xkx k () PkIKkHk () Pk IKkHk ()TKkRkKk+ =
PAGE 72
62The gain that minimizes the mean-square estimation error is found by taking the derivative of the general expression above with respect to K, setting it equal to zero, and solving for K. The resulting gain is re ferred to as the kalman gain. [B-11] Implementing The Kalman Filter The covariance matrices for both the system noise (w) and the me asurement noise (v) are given by: [B-12] [B-13] The kalman filter is executed in a loop wh ich involves calculati ng propagation error (P) and updating the estimate from a measured value (z). The first step is to project ahead and calculate the state es timates and errors. [B-14] [B-15] where u is the control effort a nd w is the process noise. Next the kalman gain is calculated using the predicted error () in the system and the covariance of the measurement noise (). The predicted error is di vided by itself plus the measurement error, which has the effect of reducing the kalman gain as the predicted error reduces. [B-16] The current estimate is then updated by the weighted (kalman gain) difference between the measured data and the noiseless model. Notice that if there is no noise or disKkPk Hk THkPk Hk TRk+ ()1 = Ewkwk T[] QkGkQkGk T== Evkvk T[] Rk= x k 1 + x kBkuw ++ = Pk 1 + kPkk TQk+ = PkRkKkPk Hk THkPkHk TRk+ ()1 =
PAGE 73
63turbances in the system and all dynamics ar e properly modeled, then the kalman gain is multiplied by zero. [B-17] From here on out, we will a ssume that there is no direct transmission of the control effort to the output of the system, thus . Table B-1: Description of kalman filter variables. Variable Description Vector containing the current state and parameters at time k. Vector containing the current stat e and parameter estimates at time k before updating the error between and . Vector containing the state and para meter estimates at the next time sample. Matrix containing the current er ror covariance for the states and parameters. Matrix containing the current er ror covariance for the estimated states and parameters. Vector containing the current meas ured states which contain noise. Continuous and discrete output matrix which contains the noiseless connections between the states and the output of the system at time k. Continuous and discrete matrix containing the error covariance of the measurement noise for the system. Continuous and discrete matrix containing the error covariance of the process noise for the system. Continuous and discrete state transition matrix. Continuous and discrete gain matrix. Kalman gain matrix which weight the amount of influence on the error between system model () and the measured data (). Process noise. Measurement noise. x kx k KkzkHkx k Dku () + = Dk0 = xkx k xkzkx k 1 + PkPk zkHHkRRkQQkF kBBkKkx k zkw v
PAGE 74
64 Extended Kalman Filter Design The extended kalman filter is based on the stan dard kalman filter, but with a first-order Taylor approximation of both the state transition matrix and the observations equations about the current estimated stat e trajectory. This allows nonlin ear equations to be used in the kalman filter, which are of the form: [B-18] [B-19] The non-linearity may enter into the equations either in the dynami cs of the system or in the observation of the stat es. Thus the state transition matrix and observation matrix become: Figure B-1. Kalman filter ing process at work, ta king noisy input measurements and producing filtered output measur ements. Note that the initial values of the state estimate () and error covariance () are zero. Compute Kalman Gain: KkPk Hk THkPk Hk TRk+ ()1 = Project Ahead: x kx kBu + = Pk 1 + kPkk TQ + = Update estimate with measurement : zkx kx k KkzkHkx k () + = Compute error covariance for updated estimate: PkIKkHk () Pk = Enter prior estimate and its error covariance x 0 P0 Noisy input Filtered outputx0 P0x · fxut ,, () Gwt () + = yhxt , () vt () + =
PAGE 75
65[B-20] [B-21] where both of these terms are now the Jacobian matrix with respect to x. [B-22] Augmented Kalman Filter The kalman filter can be used for esti mating unknown parameters. This can be accomplished by augmenting the state v ector (i.e. adding the parameters to be es timated in to the state vector) and modifying bot h the state transition matrix and the observation matrix. The new state transition matrix will take the form: [B-23] where is the state transition matrix associ ated with the dynamics of the system to be controlled. is the coupling between th e system and the parameters to be estimated. Finally, is the m odel of the estimated parame ters. There are several models that can be used for the augmented parameters. Augmenting a kalman filter incr eases the numerical complexity of the filter, thus it is desirable to limit the paramete rs to be estimated in orde r achieve good run-time performance. The augmented kalman filter is limited by its observability to the number of A x fxut ,, () = H x hut , () = x f x1 f1x2 f1 x1 f2x2 f2 = x h x1 h1x2 h1 x1 h2x2 h2 = AsystemAcoupling0 Aaugment AsystemAcouplingAaugment
PAGE 76
66parameters it can estimate. Thus it is importa nt to check the observa bility of the kalman filter prior to attempting to estimate parameters otherwise the estimated parameters could diverge greatly from the real va lues and lead to instability. Estimation Models The models used for estimation are applicati on specific, thus there is no general model that is used in every situat ion. Shown below are several diff erent ways to model parameters that are being estimated. Controllability and Observab ility of the Kalman Filter The contollability and observability of a system is important since you cannot control what is not observable nor what is not controllable. If a sy stem is determined to not be controllable, then no matter wh at type of contro ller you attempt to implement, you will not be able to control the system. For this section we wi ll use the standard definition of a state space system, shown below. [B-24] where x is the state vector and u is the control effort. Table B-2: Various models us ed in the kalman filter. Type Model Description Random ConstantA parameter that is modelled as a random constant has a fixed, but random amplitude with the initial conditions as a Gaussian with zero mean and a constant variance. Random Walk A random walk estimation model varies from one integration step to another. Here the parameters are modelled as white noise with zero mean and the appropriate mean-squared values. Markov ProcessThe Markov process model for parameter estimation allows estimates from one integration step to the next to be exponentially correlated. A 0 = Arandnvar () = Aeat= x · AxBu + =
PAGE 77
67 Controllability The controllability matrix for a linear system of size n is defined below. In order for a system to be fully controllable, the c ontrollability matrix must be full rank. [B-25] Observability The observability matrix for a linear system defines the st ates that can be observed by a control system. Thus for a system to be fully observable, it must have full rank of the observability matrix. [B-26] McBABA2B An 1 B = MoCTATCTA2 TCT An 1 () TCT=
PAGE 78
68 REFERENCES [1]Sukkarieh, S., Low Cost, High Integrity, Aided Inertial Navigation Systems for Autonomous Land Vehicles, Ph.D. Thes is, University of Sydney, March 2000. [2]Mandapat, R., Development and Evalua tion of Positioning Systems for Autonomous Vehicle Navigation, MS Thesis, Un iversity of Florida, December 2001. [3]Bennamoun, M., Boashash, B., Faruqi, F. and Dunbar, M., The Development of an Intergrated GPS/INS/Sonar Navigation System for Aut onomous Underwater Vehicle Navigation, 1990 IEEE Symposium on Aut onomous Underwater Vehicle Technology, Washington, DC, pp. 256-261, June 5-6, 1990. [4]Ohlmeyer, E., Pepitone, T., and Miller, B., Assessment of Integrated GPS/INS for the EX-171 Extended Range Guided Muni tion, July 2000, http://www.aerospacetechnology.com/downloa d/paper3.pdf, 1/1/2002. [5]Kariya, S. and Kaufman, P., New Tec hnology Transforms Tactics in Afghanistan, IEEE Spectrum, Vol. 39, No. 4, April, 2002, pp. 30-32. [6]Dana, P. H.,GPS Overview, May 2000, http://www.colorado.edu/geography/gcraft/ notes/gps/gps.html, 1/1/2002. [7]Mehaffey, C., Garmin Users Manual, April 2000, http://celia .mehaffey.com/dale/ wgarmin.htm, 1/1/2002. [8] Federal Aviation Administration, Satel lite Navigation Product Teams, July 2001, http://gps.faa.gov, 1/1/2002. [9]Jack Yeazel, GPS WAAS Operation: Your Questions Answered, July 2002, http:// www.gpsinformation.net/ waasgps.htm, 1/1/2002. [10]Nebot, E., and Durrant-Whyte, H., Initi al Calibration and Alignment of Low-Cost Inertial Navigation for Land Ve hicle Applications, Journal of Robotic Systems, Vol. 16, No. 2, February, 1999, pp. 81-92. [11]King, A. D., Inertial Navi gation -Forty Years of Evol ution, GEC Review, Vol. 13, No. 3, 1998, pp. 140-149. [12]Brown, Robert and Hwang, Patrick, Introduction to Random Signals and Applied Kalman Filtering, Third Ed., John Wiley and Sons, New York, 1997.
PAGE 79
69[13]Chatfield, A., Fundamentals of High Accuracy Inertial Navigation , AIAA, Inc., New York, 1997. [14]Rogers, R. M., Applied Mathematics in Integrated Navigation Systems , Reston, VA: American Institute of Aeronautics and Astronautics, New York, 2000.
PAGE 80
70 BIOGRAPHICAL SKETCH Kevin J. Walchko was born on 1 Nov. 1972 in Sarasota, FL, and attended the University of Florida starting in the fall of 1991. Afte r several years, a B.S. degree in mechanical engineering was earned in 1997. His college was paid for through the Montgomery G.I. Bill during his 7 years of service in the Florida Army National Guard as a Light Cavalry Scout. Kevin next received a masters degree in spring of 1999 for his work with NASA and satellite attitudes control. Most of th e research Kevin conducte d during this time was in controls and dynamics. Specifically, the rese arch involved intellig ent controls such as fuzzy logic and neural networ ks. Continuing his work with NASA, Kevin stayed at the University of Florida for a Ph.D in mechanical engineering which is expected in the spring of 2003. During his time in graduate school, Kevin also fell in love and finally married Nina C. Massie on 20 October 2001. They are currently e xpecting the arrival of their first child sometime in the beginning of 2003. Kevin has also conducted much research in robotics and autonomous vehicles. He has been a main member of the autonomous subm arine team, Subjugator, for several years. Working closely with several members of the Machine Intelligence Laboratory, in the Electrical and Computer Engin eering Department at the Univer sity of Florida, he concurrently completed a masters degree in elec trical engineering dur ing the summer of 2002 which involved inertial navigation.