Home

Multi-rate Sensor Fusion for GPS Navigation Using Kalman Filte

1. ste bee te tee ree 67 CONCLUSIONS FURTHER RESEARCH eerte 72 8 1 FURTHER RESEARCH 955 72 9 2 CONCEUSIONGG s eof t Lame that ette tese eter ete ttes PE ENS 74 AT HISTORY tee eee e pene OE dotem e A 2 OVERVIEW A 3 SYSTEM SEGMENTS A 4 DIFFERENTIAL GPS APPENDIX B GEODETIC TO GROUND CONVERSION eere 81 APPENDIX C MOTOROLA GPS RECEIVER MESSAGES 0 0 87 APPENDIX D PROGRAM LISTINGS 09 Dil DOS PROGRAM LISTINGS sii inet oe toe et e eua Eee Eb en 89 D 2 WINDOWS PROGRAM LISTINGS 110 iv List of Figures 3 1 3 2 3 3 3 4 3 5 3 6 3 7 3 8 3 9 4 1 4 2 4 3 4 4A 4 4B 4 5 4 6 4 7 4 8 4 9 5 1 5 2 53 54 5 5 5 6 5 7 5 8 5 9 5 10 5 11 5 12 5 13 5 14 5 15 5 16 5 17 5 18 5 19 5 20 5 21 6 1 6 2 6 3 6 4 6 5 6 6 6 7 6 8 6 9 TA B 1 D 1 1997 FORD TAURUS TEST VEHICLE 5 SIGNAB CONDITIONING BOARD 2 25 5 c eee eoe cocto ABS SENSOR CONNECTION FOUND UNDER THE
2. 89 Chapter 1 Introduction 1 1 Motivation For several years Global Positioning Satellites have orbited the Earth to provide absolute positioning on land on sea and in the air Millions of GPS receivers are in use around the planet in applications ranging from remote desert research to underwater cartography to simple recreation Whatever the application the main function of the GPS receiver remains constant to obtain absolute position measurements anywhere on the globe One major hurdle to GPS inherent in its method of operation is blockage of satellite reception Tall buildings bridges high mountains and common foliage overhead can block satellite reception An alternative to GPS navigation is an inertial navigation system INS INS is the application of sensors such as gyroscopes and accelerometers to maintain relative position information However inertial navigation has its drawbacks Over a substantial amount of time INS errors tend to accumulate unbounded and result in position estimates that deviate from the actual position For these reasons methods have been devised which fuse the GPS position measurements and inertial navigation measurements to provide a best estimate of position at any given time The purpose of this thesis is to examine various possible solutions for this system and to present the methods by which one might implement such a system Multiple sensor configurations are presented along
3. 15 AD SENSOR BYPES ucc ede idet oN tek eet ee RE edt 15 4 21 DISTANCE ENCODERS edt 16 42 TEACHOMETERS save sabe DUE 17 2 9 AGCEEBROMETERS 5 5 4 5 556 18 ZA AST SENSORS S eol itte AME te at ss 18 42 5 GYROSCOPES 5 4 ere Ur rere ve 20 APD STEERING EOSTTION S bbc o m S TA EE S MM P M e Sanne ema 20 4 3 SENSOR CONFIGURATIONS 21 4 3 1 CONFIGURATION 1 4 3 2 CONFIGURATION 2 4 3 3 CONFIGURATION 3 4 3 4 CONFIGURATION 4 453 5 CONFIGURATION CHOICE tec cere Ee 26 gt L GENERAL 555 Peso 27 5 2 GEODETIC TO GROUND n eene nnne nenne ene n sen rh sene e ese er 29 5 3 KALMAN FILTER FOR IINS e ree te ree A eter ee de EF EXT 31 5 4 RULE BASED FUSION OF INS AND 5 34 5 9 FUZZY FUSION OE INS AND GPS tenet reet ee ro tene Lese OE euge UE UN 37 5 6 FUSION PARAMETER OPTIMIZATION 2000 111
4. acquisition EF 88 acquisition Computer comm cables GPS Receiver Figure 3 9 The entire data collection assembly 13 3 6 Host Computer The computer we used for data collection was a Fujitsu Pentium 133 with 16 megabytes RAM running Windows 95 a typical laptop computer During operation the only external connections required were to the GPS receiver which used a serial port and to the DaqBook which used the parallel port Using a laptop made the data collection easy By using long serial and parallel cables from the trunk to the front seat the operator could sit in the passenger seat and watch all of the data collection take place in real time This proved very useful in the few occasions when something would happen such as a cable coming loose because we could detect and correct this problem immediately rather than waste time on a bad data collection 14 Chapter 4 Modeling Sensor Fusion Algorithms In order to produce a good filtering algorithm with which to work we must start with accurate dynamical models describing the system This chapter covers many alternatives and groups of alternatives such as various sensor choices that may be presented to the designer of a similar navigation system For each alternative we will present the dynamical equations that relate it to the system s state variables In addition we will discuss the viability of actually implementing t
5. 41 5 6 MANUAL PTIMIZATION esteso etuses 42 910 2 ITERATIVE HILE CEIMBING s EDS ES 42 5 6 GENETIC AEGORITHMS inzeeenenenenenesencenenenoeeteopenopeneleneeenee enemies 43 5 7 MAP MATCHING 5056 debaduvefevedune 46 6 1 SOFTWARE STRUCTURE AND DATA 000400000 000 51 6 2 DOS SOFTWARE DEVELOPMENT 000 52 6 2 1 PROGRAMMABLE INTERVAL 53 6 2 2 COM INTERRUPT PROGRAMMING 56 02 3 V GA OUTPUTS i eliotn eh Met cul AMES 58 6 2 2 DATA VISUALIZA TIONss sccsseedesedasadesedstadeascoseseasdesedesededededededsdesedosedesedededededededededesaceascoss senedsaadeaadseadsaecsancs 6 3 WINDOWS SOFTWARE DEVELOPMENT 6 3 MULTIALHREADING zieht eee eret eere 6 3 2 MULTIMEDIA TIMER AND REAL TIME 64 6 3 33S BRIAE COMMUNICATIONS E e IU URN URN M RUE TVUS E RUM RU RU RURSUS RN A S VERUS 65 RS IAN CBE O RECEN 66 6 3 5 EIETER IMBEEMENTA TION nire tees pestes peste e rd 67 6 3 6 DATASVISUATIZATION s c
6. GPSHeadWeight similar to GPSPosWeight but with different membership values GPSToCurrDist SetValue SOR m_GPSLongitude xkm1 1 SOR m GPSLatitude xkm1 2 GPSCurrVelocity SetValue m CurrGPSData velocity double LastGoodGPSGPSDist 5 GPSLongitude m LastGoodGPSLong SOR m GPSLatitude m LastGoodGPSLat double LastGoodGPSOdoDist SOR m LastGoodGPSDLat SOR m LastGoodGPSDLong DistRatio SetValue LastGoodGPSGPSDist LastGoodGPSOdoDist NumNoGood SetValue m_NumGoodNoWeight determine m_GPSHeadingWeight and m_GPSPosWeight based on fuzzy membership values double memval if GPSToCurrDist is SMALL and GPSCurrVelocity is FAST then GPSPosWeight is LARGE memval MIN GPSToCurrDist GetMembership SMALL_3 GPSCurrVelocity GetMembership FAST 3 GPSPosWeight SetMembership LARGE 5 memval GPSHeadWeight SetMembership LARGE 5 memval if GPSToCurrDist is MED and GPSCurrVelocity is FAST then GPSPosWeight is MED 116 if GPSToCurrDist is SMALL GPSCurrVelocity is MED then GPSPosWeight is MED memval MAX MIN GPSToCurrDist GetMembership MED_3 GPSCurrVelocity GetMembership FAST 3 MIN GPSToCurrDist GetMembership SMALL 3 GPSCurrVelocity GetMembership MED 3 GPSPosWeight SetMembership MED 5 memval GPSHeadWeight SetMembership MED 5 memval if GPSToCurrDist is MED and GPSCurrVelocity is MED then
7. 4 3 1 Configuration 1 GPS Receiver Steering Position and Odometer GPS Receiver Steering Position Sensor Fusion System Figure 4 6 Configuration 1 block diagram In this configuration the odometer signal is a series of pulses where each pulse represents some fixed distance covered Thus the odometer provides us with a direct measurement of distance covered Either an absolute encoder or a potentiometer can measure the steering position In this case the position of the front wheels is of interest so we assume a bicycle model where the front and rear wheels are each considered a single wheel at the midpoint of the two axles Let the state of the vehicle be represented by Q v x and y give the location of the rear axle midpoint gives the angle of the vehicle body with the horizontal gives the steering angle with respect to the vehicle s body and v is the forward velocity of the rear wheels of the vehicle See Figure 4 1 21 The dynamic equations for this vehicle model are 0 0 vtan v QX METERS PER TICK where is the odometer measurement in ticks and METERS PER TICK is conversion factor which changes ticks to meters The value of METERS PER TICK depends on the number of ticks per wheel revolution and the circumference of the wheel which may change but we will assume to be a constant We must use the discrete form of these equations to implem
8. free matrix Pkm1 1 8 1 free matrix R 1 2 1 2 free matrix 0 1 8 1 8 free matrix mtemp1 1 8 1 8 free matrix mtemp2 1 8 1 8 free matrix mtemp3 1 2 1 8 free matrix mtemp4 1 2 1 2 free matrix mtemp5 1 2 1 2 free matrix mtemp6 1 2 1 2 free matrix mtemp7 1 8 1 2 free matrix mtemp8 1 2 1 8 free vector mtemp9 1 2 free vector mtemp10 1 2 free vector mtemp11 1 8 free vector mtemp12 1 8 free matrix mtemp13 1 8 1 8 free matrix mtemp14 1 8 1 8 m FilterInitialized FALSE FilterlHz is called to perform the older rule based sensor fusion when new GPS data 27 is received Either FilterlHz or FuzzylHz is called depending the user selection void CPostViewApp FilterlHz void kd we just received new GPS data and wish to use rule based fusion roughly 1 Hz if m fInitGPS amp amp m GPSGood m_fInitGPS TRUE just got first good GPS value gt initialize position etc in state matrix double newhead 90 0 m_CurrGPSData heading newhead fmod newhead 360 0 360 0 xkm1 1 m_GPSLongitude xkm1 2 m GPSLatitude xkm1 3 newhead calculate local meters gt msec lat long here first convert latitude msec to radians double tlat DEG TO RAD m GPSLatitude 3600000 0 double tsin sin tlat meters to msec long pow 1 E2 tsin tsin 0 5 0 03083070 cos t1at meters to msec lat pow 1 E2 tsin tsin 1 5 0 030715169 m LastGoo
9. to to set mask read palette to write palette to send data to value video video video port video port VGA screen Text mode Character Character port port structure containing palette color info structure containing inertial sensor info structure representing a single point structure representing a road segment for map drawing COM1 I O character buffer Line status register Interrupt enable register Control register Timer ISR jump vector 1 ISR jump vector Default PIT frequency 90 Maplog c incl incl incl incl incl incl incl incl incl incl incl incl lude lt stdio h gt lude lt string h gt lude lt stdlib h gt lude lt bios h gt lude lt dos h gt lude lt mem h gt lude lt conio h gt lude lt alloc h gt ude lt math h gt lude lt string h gt lude drgps h structures for storing GPS info lude daqbook h functions for accessing the DaqBook lude maplog h structures and defines for this program define NOBOOK define for testing when no daqbook connected Function prototypes void void void void void void void void void void void void void void void void void void void void void void void void void void void SetTimer void interrupt far void int SetComl void interrupt _ far void CleanU
10. curr_guage 1 amp amp color BACKGNDCOL tcol BORDERSELECTCOL else tcol color h_line SBULX SENOFFX i SENSPACE 1 SBULX SENOFFX SENWIDTH i SENSPACE SBULY SENOFFY tcol vscreen h_line SBULX SENOFFX i SENSPACE 1 SBULX SENOFFX SENWIDTH i SENSPACE SBULY SENOFFY SENHEIGHT tcol vscreen FY SBULY SENOFFY SENHEIGHT SBULX SENOFFX i SENSPACE 1 tcol vscreen FY SBULY SENOFFY SENHEIGHT SBULX SENOFFX i SENSPACE SENWIDTH tcol vscreen 1 1 v_line SBULY SENO 1 v_line SBULY SENO 1 v_line SBULY SENOFFY int 5000 0 sen_high i SENHEIGHT 5000 0 SBULY SENOFFY int 5000 0 sen_low i SENHEIGHT 5000 0 SBULX SENOFFX i SENSPACE ticol vscreen plot a string to the output window starting at position sx sy void blit_string int sx int sy char str unsigned int color unsigned char far vscreen int i 0 while str i 0 blit char sx sy str i color vscreen Sxt 8 itt draw bar representing one of the sensor values void draw_sensor_vals unsigned int color unsigned char far vscreen int i offset for 1 0 1 lt 5 1 if sen_val i lt sen_high i amp amp sen_val i gt sen_low i offset int float sen high i sen val i float sen high il sen low i float 2 1 h line SBULX SENOFFX i SENSPACE 2 SBULX SENOFFX SENWIDTH i SENSPACE 1 SBULY SENOF
11. fwrite amp log data ticks sizeof unsigned long 1 gps file last gps hsec 1log data ticks fwrite amp GPS chan sizeof T POS CHAN STATUS 1 gps file output to file new gps stuff new gps 0 Q postview read GPS data from file if nexttime lt log_data ticks synchronize with inertial sensor data if nexttime GPS_chan GPS_chan2 last_gps_hsec nexttime gps_latitude long abs GPS_chan latitude degrees 60 0 GPS_chan latitude minutes 60 0 GPS_chan latitude seconds 1000 0 gps_longitude long abs GPS_chan longitude degrees 60 0 96 GPS_chan longitude minutes 60 0 GPS_chan longitude seconds 1000 if GPS_chan latitude degrees lt 0 gps latitude 0 gps latitude if GPS_chan longitude degrees lt 0 gps longitude 0 gps longitude gps valid 1 if GPS chan rcvr status amp 0x43 gps valid 0 if GPS chan rcvr status amp 0x30 gps valid new gps stuff override INS calculations if good GPS fread amp nexttime sizeof unsigned long 1 gps file fread amp GPS chan2 sizeof T POS CHAN STATUS 1 gps file 0 calculate new long lat here calc_latitude gps_latitude calc_longitude gps_longitude if novga update screen portions at staggered intervals if hsec 10 quickview if mapposx mapposy put box back at mapposx mapposy put box mapposx 2 mapposy 2 CURSBACKBOXWIDTH CURSBACKBO
12. m_pGPSFile gt Read amp m_NextGPSData sizeof T_POS_CHAN_STATUS memcpy amp m_CurrGPSData amp m_NextGPSData sizeof T_POS_CHAN_STATUS m_CurrGPSTime m_NextGPSTime m GPSLatitude int m_CurrGPSData latitude seconds m_CurrGPSData latitude minutes 60 0 abs m_CurrGPSData latitude degrees 3600 0 1000 0 SGN m_CurrGPSData latitude degrees m GPSLongitude int m_CurrGPSData longitude seconds m CurrGPSData longitude minutes 60 0 4 abs m CurrGPSData longitude degrees 3600 0 1000 0 SGN m CurrGPSData longitude degrees if m CurrGPSData rcvr status amp 0x43 110 m CurrGPSData rcvr status amp 0x30 m GPSGood FALSE else m GPSGood TRUE m pGPSPage m GoodGPSCheckVar m GPSGood m pGPSPage UpdateContents run Filterl00Hz for sensor data gt update sensor page if m pMainPage m HertzEditVar amp amp m RunCount m pMainPage m HertzEditVar 10 m pSensorPage UpdateContents else if m RunCount 50 m_pSensorPage gt UpdateContents Filter100Hz execute the inertial sensor filter if time on sensor next GPS time synchronize GPS and inertial data if m CurrSensorData ticks m NextGPSTime copy m_NextGPSData to m_CurrGPSData memcpy amp m_CurrGPSData amp m_NextGPSData sizeof T_POS_CHAN_STATUS m_CurrGPSTime m_NextGPSTime update m_GPSLatitude m_GPSLongitude m_GPSGood m GPSLatitude int m_CurrGPSData latitude se
13. void fill_screen unsigned int color unsigned char far vscreen int i for 1 0 1 lt 200 1 _fmemset char vscreent i lt lt 8 i lt lt 6 color 320 put a single pixel on the screen void blit_bit int x int y unsigned int color unsigned char far vscreen vscreen lt lt 8 y lt lt 6 x color set the screen to vga mode void void asm pusha mov ax 0x0013 int 0x10 popa set the screen to text mode void set_text void asm pusha mov ax 0x0003 int 0x10 popa setup the palette colors for this application void set pallette void RGB color color color red 0 color green 0 color blue 0 Set_Palette_Register BLACK amp color 103 color red 255 Set_Palette_Register RED amp color color green 255 Set_Palette_Register YELLOW color color red 0 Set Palette Register GREEN amp color color blue 255 Set Palette Register CYAN amp color color green 0 Set Palette Register BLUE amp color color red 255 Set Palette Register MAGENTA color color green 255 Set Palette Register WHITE amp color put a single colored character on the screen an location xc yc void blit char int xc int yc char c int color unsigned char far vscreen int offset x y unsigned char far work char unsigned char bit mask 0x80 work char rom char set c CHAR HEIGHT get offset pos
14. Then the system will make decisions based the membership of crisp data value in a fuzzy set In this case the fuzzy sets are defined to be Close and Far Suppose that we have a distance value d The membership of 4 in the sets Close and Far can be determined as shown in Figure 5 7 Jang et al 1997 Rao 1995 T pee Qt mtt ttt tte Membership of di to fuzzy md set Far M iu d 0 65 of d to fuzzy set Close U crose d 0 35 Distance Figure 5 7 Fuzzification of a crisp data value Now suppose we also have fuzzy sets Fast and Slow which are sets based on the speed value from the GPS receiver Then we now have a set of fuzzy rules such as If Distance is Close and Speed is Fast then is Large This states that if the GPS distance is close to our estimated position and the GPS receiver measures a significant vehicle speed then we assume that the GPS data is good and set the GPS position weight to a large value Likewise we may have the following fuzzy rule If Distance is Far and Speed is Slow then V is Small This statement is essentially the converse of the previous rule That is if the GPS distance is far from our estimate position and the GPS receiver measures a low vehicle speed then we assume the GPS data is bad and set the GPS position weight to a small value Several more variables must be added to make the system robust however For example a variab
15. Where U is the gyroscope measurement in degrees second is the first 2 gt 2 i accelerometer reading in meters second and U is the second accelerometer reading in g The discrete form of these equations is shown here 25 x k 1 6 k y k 1 y k Tv k sin 6 k k 1 0 k T7U k 4 3 5 Configuration Choice In our project based on the quality of the available sensors we chose to use the second configuration In the test we performed we found that the best performance was obtained by using only the gyroscope and the odometer Depending on the application or available sensors a different configuration can be chosen 26 Chapter 5 Algorithm Extensions When designing the overall fusion algorithm we must take into consideration the multi rate sensor collection and design our algorithm appropriately That is we need to fuse inertial and GPS data when we are sampling our inertial sensors at 100 Hz and receiving GPS data at 1 Hz Here the term fusion is refers generally to the process of combining two sets of data to produce a better output Sensor fusion can be accomplished with a filter or it can be done by weighting each set of data based on a set of rules or fuzzy logic We considered a couple different methods for fusing these sets of data For example we could extrapolate GPS data at 100 Hz based on previous GPS data trends and perform sensor fusion at 100 Hz Co
16. e sin o 2 84 Substituting equations 19 23 into equation 13 we get ds daz Y do do d a sin 1 2 a cos o 1 L e sin o Reducing to _ 24 3 1 e 2 2 4 Equation 24 represents de in units of a per radian To get into meters per degree of latitude we use 2x radians 360 which results in the following ds 2x 1 e Ja 49 360 1 e sin 2 To obtain the value for we use equation 11 to compute the distance d around the earth along a line of latitude 6 de 2 COS 26 1 1 e sin 2 Because d represents the distance for a full 360 around the earth we divide equation 26 by 360 to get C2 Gx a 2ra cos 27 360 1 e sin 2 where 6 378 150 meters e 0 08181333 For our purposes we desire C and C in units of meters per millisecond instead of meters per degree Therefore we simply divide equations 25 and 27 by 3 600 000 to 85 convert each from degrees to milliseconds Substituting the values for and and converting to meters per millisecond we have 1 3 28 32 55720 1 0 0066934 sin o 2 coso 29 1 33 3392843 1 0 0066934 sin o 2 86 Appendix Motorola GPS Receiver Messages What follows is a breakdown of one input command and the resulting response from the Motorola Oncore GPS rec
17. from com port map information long num map roads num map segments char map roads ROAD SEG map segments int quickview flag set to no delay on replay of data int novga flag set to not display data graphically int tsec since gps count of time since last GPS message received int gps report idx unsigned char gps port intr set void main int argc char argv declare and initialize local data char log_file_name 40 file names char gps_file_name 40 char key_file_name 40 unsigned char OldPallette 256 3 location to store original palette LT cb initialize global data vscreen unsigned char far 0xA0000000L location of video memory rom_char_set unsigned char far 0xFOOOFA6EL rom character set 92 get_pc_time hsec start_hsec pc_seconds 100 pc_time ti_hund initial hsec count new_sense quickview mapping logging postview 0 initialize all flags to false initialize all data to default values curr_guage curr_speed 0 mapposx mapposy 0 novga gps_report_idx 0 calc_latitude calc_longitude 0 prev sec old speed old gps valid 1 old sats tracked 1 initialize output strings appropriately for 1 0 1 lt 4 1 rtstr i tsgps str il for 1 0 1 lt 8 1 pct str i rtstr 2 pct str 2 pct str 5 s tsgps str 2 2 rtstr 5 speed_str 3 lat_str 10 long_str 10 pct_str 8 tsgps_str 4 0 for i
18. 0 i lt 6 i strength i 0 for 1 0 1 lt 5 sen_high i 5000 sen_low i 0 sen_val i 0 for 1 0 1 lt 10 1 lat_str i long_str i 0 initialize map border values mapullong STMAPULLONG mapullat STMAPULLAT maplrlong STMAPLRLONG maplrlat STMAPLRLAT nexttime 0 log file gps file map file cfg file debug NULL old data NULL roads NULL map segments NULL debug fopen debug dat w extra debug info to file old data farmalloc sizeof LOG DATA OLD DATA SIZE if old_data NULL printf Error allocating memory n PYrInNGE TEXICING a ANDU goodbye 0 for i 0 i lt OLD_DATA_SIZE i old_data i log_data init data to O s ll parse command line for i 1 i argc i if strcmp argv i 1 stremp argv i L if postview logging 1 logging data acquisition mode i strcpy log file name argv i strcpy gps file name argv i strcpy key file name argv i strcat log file name log strcat gps file name gps strcat key file name key open log output file if log file fopen log file name wb NULL printf Error opening log file s for output n log file name printf Exiting xn goodbye 0 open GPS output file if gps_file fopen gps_file_name wb NULL printf Error opening gps file s for output n gps_file_name
19. 1A Parameter 1B Parameter 1C Parameter 1D Befores After Genotype 1 Parameter 1 B Parameter2D Figure 5 15 Genetic mutation of electronic genotypes By using these genetic algorithm techniques a more ideal parameter set can be obtained at the expense of the additional time required to test numerous parameter sets that are very unfit The genetic algorithm optimization method is not subject to getting stuck in local minima or maxima however it is not guaranteed to ever converge to a good solution Chambers 1995 45 5 7 Matching Although the fuzzy fusion of GPS and INS data results in very accurate position estimates in most environments we are often interested in the performance of the system in an urban environment such as downtown New York City In this type of environment the GPS signal is often blocked or significant errors are introduced such that even the most robust of filtering algorithms will output an inaccurate position In addition the need for accurate position estimates is even greater in a city environment due to the close proximity of adjacent roads Thus we introduce a method of increasing the ability of the system by using prior knowledge of a given area Map matching is simply a method of using stored information about a region to improve the ability of a position estimation system to handle errors Essentially a known map re
20. 2 Filterl00Hz is called whenever a new inertial sensor value is acquired This is independent of the method of GPS INS fusion being used This is primarily just a Kalman filter void CPostViewApp Filterl100Hz void we just sampled our sensors at 100 Hz if m_fInitGPS perform extended kalman filter interation gt put measurement variables into yk yk 1 dtheta and yk 2 odo measure double dthetal m GyroCenter m CurrSensorData gyrol m MvoltPerDegree double odo measure m CurrSensorData odometer total old odo total m TicksPerMeter if odo_measure lt 0 odo measure m MaxDistPer100 odo measure 0 old odo totalem CurrSensorData odometer total if odo measure 0 odo zero count tt else odo zero count 0 if odo zero count 20 m GyroCenter m GyroCenter 0 984m CurrSensorData gyrol 0 02 if ABS m CurrSensorData steer m SteerCenter 50 dthetal dthetal ABS m CurrSensorData steer m SteerCenter 100 0 yk 1 dthetal yk 2 odo measure gt generate system transfer by linearizing non linear functions Ak Ak 1 6 2cos TO RAD xkm1 3 meters to msec long Ak 2 6 sin DEG RAD xkm1 3 meters to msec lat m LastGoodGPSDLat Ak 2 6 odo measure m LastGoodGPSDLong Ak 1 6 odo measure generate measurement transfer by linearizing non linear functions Ck predict error covariance 1 0 m
21. DC to 110 volt AC voltage inverter so that we could operate the VCR and laptop computer for extended periods of time Figure 3 1 1997 Ford Taurus test vehicle 3 2 Dead Reckoning Sensors In order to collect odometry data from the vehicle we wanted to obtain a direct measurement of distance that the vehicle had traveled Rather than attaching an additional encoder somewhere on the drive train we used data outputs that Ford has already provided The method we tried was simply to use the output from the odometer directly The output from this connection needed to be conditioned before it was input into the data acquisition board We added a simple circuit that railed the voltage from 0 to 5 volts when it crossed a voltage threshold This method worked well when the vehicle was moving sufficiently fast but generally did not work below speeds of a few miles per hour This is because of the nature of the technology that the sensor is based upon The signal is produced as a magnetic field rotates at sufficient speed near a coil of wire inducing a current in that wire Because of the type of sensor used to produce the signal it does not work well at low speeds It was decided that this was inadequate for our purposes so we chose another method of odometry collection Odometer signal 12 Volt conditioning circuit power input 5 Volt regulator Figure 3 2 Signal conditioning board The second method provided better res
22. LEFT REAR SEAT MOUNTED STEERING POTENTIOMETER ee DIAGRAM OF ACCELEROMETER ORIENTATIONS ON SENSOR BOARD EXAMPLE PLOT OF INERTIAL SENSOR 5 0 CLOSE UP PICTURE OF THE 20 0000000 0 0 00 0000 0000000 555525511521 MOTOROLA VP ONCORE GPS RECEIVER FROM MOTOROLA ONCORE USER S GUIDE 12 THE ENTIRE DATA COLEECTION ASSEMBLY eere eeu eeu eee e Vou ue uen Reg eremo eu en De eue a Mo EEUU 13 SIMPLE 2D DYNAMIC MODEL OF VEHICLE 2 2 24 1 2 0 0000000000000002000 rr rr rene 15 TYPICAL SMALL QUADRATURE ENCODER eene enne nene eene e nenne nene nennen nee enne sees esee eese nene nennen eene ene 16 OBTAINING DIRECTION FROM QUADRATURE ENCODER 17 VEHICLE WITH SIDE TILT RP 19 FRONT TIET 3 355300 ave goa vasa aga eun eus v Va M ONES 19 A TYPICAL LIQUID FILLED TILT SENSOR 19 CONFIGURATION 1 BLOCK 2 21 CONFIGURATION 2 BLOCK DIAGRAM aee eoe Veo eoe aeo eoe ooo eU Ee ee 23 CONFIGURATION 3 BLOCK DIAGRAM 5 0 aede pete ose o egeo eue ede 24 CONFIGURATION 4 BLOC
23. Prine tine goodbye 0 open key press output file if key file fopen key_file_name wb NULL printf Error opening key file s for output n key_file_name printi Exiting z Vn goodbye 0 93 else goodbye 1 else if strcmp argv il p stremp argv i P if logging postview 1 postview replay mode i strcpy log file name argv 2 strcpy gps file name argv 2 strcat log file name log strcat gps file name gps open log input file if log file fopen log file name rb zNULL printf Error opening log file s for input Mn log file name pr intf Exiking 2 2 xg goodbye 0 open GPS input file if gps_file fopen gps_file_name rb NULL printf Error opening gps file s for input n gps file name printf Exiting goodbye 0 else goodbye 1 else if strcmp argv i m stremp argv i M char mapfilename 20 itt strcpy mapfilename argv il strcat mapfilename map open map input file if map file fopen mapfilename rb NULL printf Error opening map file s for input n mapfilename priuntf Exiting Xn goodbye 0 fread map name 24 1 map file fread amp num map roads sizeof long 1 map file fread amp num map segments sizeof long 1 map file allocate memory for ro
24. We are sampling data at 100 Hz in this project and each sample is 14 bytes of data resulting in 11 2 Kbytes sec of data transferred Any commands sent to the board must also be considered but we can see that the board can easily handle the data rate we desire More about the data samples will be presented in the software chapter of this thesis DaqBook 1994 The data acquisition board can be used in several different modes but we essentially used only two polled output and timed output In DOS based programming we are less encumbered by delays introduced by the operating system OS on I O operations such as parallel port reads and writes For this reason we are able to use polled output from the DaqBook which is substantially easier to implement A 100 Hz loop is implemented using interrupt based timing in the host computer and in each loop we send a request and receive a response with negligible delay When programming under Windows 95 which is not truly a real time OS we found that we could not use the simple polled response method that worked with DOS After a little work we found that the DaqBook could be set to automatically sample data at a specified rate This rate is maintained by internal timing circuitry so our program did not need to initiate each sample The program merely grabbed the data sample from the parallel port at the appropriate time More details about the implementation in the programs are presented in the c
25. at 1 Hz it actually runs slightly slower than 1 Hz It runs even slower as the desired frequency is increased Since we wanted to run at 100 Hz this problem was unacceptable Attempting to run at 100 Hz resulted in running at a mere 17 Hz Although this timer is simple to implement we turned to something a little more satisfactory the multimedia timer The multimedia timer is not as simple to use as the CWnd timer is but it provides higher accuracy at much higher speeds The multimedia timer uses a separate thread to generate timed function calls in the application The handling of the thread is done internal to the multimedia function calls but the programmer must properly set up and shut down the timer using the multimedia functions timeSetEvent and timeKillEvent respectively Also the multimedia library mmsystem 1lib must be linked into the application and the header file mmsystem h included in the source code Despite the additional trouble the multimedia timer can generate function 64 calls at a steady 100 Hz so we were able to capture data at the desired sampling rate For more information about the multimedia timer functions see timeSetEvent in the MSVC help files 6 3 3 Serial Communications For one used to serial communications in a DOS environment serial communications under Windows 95 is a very tricky thing The designers of Windows 95 have managed to abstract serial I O to be a form of file I O wher
26. found do not collect data from it gps found 20 get the time from the BIOS void get_pc_time void 104 gettime amp pc time hour pc time ti hour min pc time ti min sec pc time ti sec pc seconds hour 3600 min 60 sec get the odometer reading from the DaqBook and compute relative distance void get odometer count void unsigned int count unsigned long new total daqCtrMultCtrl DmccSave 1 1 0 0 0 daqCtrGetHold 2 amp count new total count new total 0 100001 daqCtrGetHold 1 amp count new total count odometer sample new total odometer total odometer total new total detect a user key press void get user key void if I kbhit O user 0 return user_key getch decode the raw GPS message into useable data void Pos_Status_Data_Decode unsigned char Status_Message T_POS_CHAN_STATUS pos_chan char scan_mode UNSIGNED_ONEBYTE i UNSIGNED_ONEBYTE tempchar UNSIGNED_FOURBYTE tempu4byte FOURBYTE temps4byte double degrees minutes int message_posn 0 skip first 4 bytes Ba message_posn 4 read and scale the rest of the data pos chan month Status Message message posn t pos chan day Status Message message posnt t tempchar Status Message message posn t pos chan year tempchar lt lt 8 Status Message message posn t pos chan hours Status M
27. inertial and GPS data values For our project we chose to perform defuzzification using the centroid method which is a method that is very commonly used in fizzy logic applications The centroid method involved finding the centroid of the shaded area in Figure 5 9 The crisp output of the system is actually the x axis value corresponding to the centroid of the shaded area 1 Centroi Y ros Figure 5 10 Centroid representing crisp output of the system In our implementation of fuzzy logic for sensor fusion we chose to keep the set of variables small and the rules simple The intention was to explore the possibility of enhancing fusion performance using fuzzy logic techniques The fuzzy logic system we created used three variables as inputs and produced one output The three inputs were 40 Distance The distance from current position estimate to the GPS position Velocity The value of the vehicle velocity returned by the GPS receiver NumNoGood The number of fusion iterations in which the GPS data has been determined to be bad because it was too far from the current position estimate This variable is used for error recovery in the event that our position estimate becomes seriously skewed from the actual vehicle position The single output of the fuzzy fusion was GPSWeight the weight of the GPS data for fusion which ranges from 0 to 1 The weighting of the inertial data is GPSWeight In our approach t
28. int char unsigned int unsigned char far draw strength border unsigned int unsigned char far draw strength vals unsigned int unsigned char far put box int int int int unsigned char unsigned char far get box int int int int unsigned char unsigned char far new gps stuff void int sgn int POINT 11 to screen POINT int in view POINT void goodbye int Global data void void interrupt _ far BIOSTimerHandler void old timer ISR pointer interrupt _ far BIOSComlHandler void old coml ISR pointer 91 volatile long counter clock_ticks timer ISR counter variables unsigned char far vscreen pointer to the video screen in memory unsigned char far rom_char_set pointer to characters in memory FILE log_file output log file FILE gps_file output gps file FILE map_file input map file FILE cfg_file input configuration file FILE key_file output key press file FILE debug output debug file int logging postview mapping running indicator flags for program flow volatile int new_sense 01 second elapsed flag struct time gps time time of a GPS capture struct time pc time time of a sensor capture unsigned long hour min sec pc seconds start hsec volatile unsigned long hsec timer ISR counter variables volatile int new gps new GPS data flag unsigned long pct val old pct
29. num of visible sats t num of satellites tracked For each of six receiver channels i satellite ID m channel tracking mode 0 Code Search Code Acquire 2 AGC Set 3 Freq Acquire 4 Bit Sync Detect s signal strength 0 PDOP in 3D 1 HDOP in 2D 0 12 0 0 37 0 8 5 Message Sync Detect 6 Satellite Time Avail 7 Ephemeris Acquire 8 Avail for Position 0 255 number proportional to signal to noise ratio d channel status flag Each bit represents one of the following msb Isb End of channel dependant data S receiver status message msb lsb C checksum Bit 7 Using for position fix Bit 6 Satellite momentum alert flag Bit 5 Satellite anti spoof flag set Bit 4 Satellite reported unhealthy Bit 3 Satellite reported inaccurate Bit 2 Spare Bit 1 Spare Bit 0 Parity Error Bit 7 Position propagate mode Bit 6 Poor geometry DOP gt 20 Bit 5 3D fix Bit 4 Altitude hold 2D fix Bit 3 Acquiring satellites position hold Bit 2 Differential Bit 1 Insufficient visible satellites lt 3 Bit 0 Bad almanac 88 Appendix D Program Listings D 1 DOS Program Listings The DOS program that was written for this thesis was used primarily to collect GPS and sensor data and store it to disk in real time This program also used simple VGA graphics output to display information about GPS readings and sensor values and to display the curr
30. our desired 100 Hz Therefore we must reprogram the PIT chip to generate ticks at 100 Hz The following four statements are all that are required to set the PIT chip frequency counter long PIT_FREQ frequency calculate new counter value outp 0x43 0x34 send command to set new value to the PIT outp 0x40 counter 256 send low byte of new counter value outp 0x40 counter 256 send high byte of new counter value counter is the value which is loaded into the PIT chip and serves as a divisor of the PIT frequency which is 1234DD hex or 1193181 decimal We desire a frequency of 100 Hz so counter is 11931 decimal or 2E9B hex The function outp portid value simply outputs the byte value to the hardware port portid Yet another problem arises when we do this If we reprogram the PIT chip to generate interrupts at 100 Hz and then make our own interrupt service routine ISR to handle the interrupts we will prevent the system timer from being updated This is resolved by calling the system timer interrupt manually at roughly 18 2 Hz The following code segment shows our entire timer ISR Note that we do not do any data collection screen writes or file I O in the ISR We only increment a number set a flag and call the system timer routine if necessary This is because interrupt service routines should be kept short and simple whenever possible to avoid problems void interrupt far Handler void hsec
31. section view or ellipucal required The reason that this relation is needed is so that the data obtained from the GPS receiver which is in geodetic coordinates latitude and longitude can be processed along with data obtained 29 from the odometer and inertial sensors which is most easily converted into relative Y meters Given a particular location on the surface of the Earth we would like to know how many meters of north south or east west travel correspond to how many degrees of latitude or longitude respectively Note that assuming the Earth to be a perfect ellipsoid with a fixed equatorial radius a and eccentricity e these conversion rates are dependent only upon the current latitude measurement The conversion rates that were used in our software and algorithms were taken from calculations performed by Gene Felis Electronics Engineer NUWC Div Keyport in 1976 He based his calculations on the elliptical world model with geodetic latitude as presented on pages 26 29 of Methods of Orbit Determination by P R Escobal 1965 The formula derivations are rather complex and have been included in Appendix B of this thesis The end results of the derivations are shown here The constant Ci which is the number of meters north to south corresponding to the change of degree in latitude is calculated as follows 2 1 y 360 3600 1 e sin 9 The constant C which is th
32. those who have never had the good fortune to do some old fashioned low level DOS programming this section may prove quite interesting We wish to present the methods by which a DOS program can be made to collect process and display sensor 52 data in exactly the manner we desire We need to make special allowances for a few parts of our program We wish to collect sensor data at precisely 100 Hz and to collect GPS data at precisely 1 Hz After some thought we realize that the GPS data should not be collected at 1 Hz but instead we want to listen at the serial communications port com port for the GPS data This is because the GPS receiver is sending data to the PC whenever it wants to essentially making it an asynchronous process to our program For this reason we cannot have our program wait on the com port because we never know how long we will be waiting Instead we would like to ignore the com port and only service it when new data arrives Thus we make use of interrupts Even if we ignore the com port completely we are faced with one serious problem How do we make sure that the main collection processing loop of the program takes place at exactly 100 Hz Again interrupts come to the rescue The following section covers the implementation of an interrupt driven program in DOS All programming was done using Borland C version 4 5 However most of the code presented here can be used with other 16 bit compilers with
33. to generate an interrupt which is then handled by our ISR whenever a data byte has arrived on the port Likewise the ISR for the com port must do more than set a flag At any given time our program is unaware of where we are in the message from the GPS receiver For this reason the interrupt handler must be able to synchronize itself with the GPS message Synchronization is accomplished by finding a known sequence of characters in the message then orienting the rest of the message by this sequence We implement this by means of input states Initially we are in state waiting for the first character in the sequence which is a character Once we get the character we advance to state 1 where we again wait for a character Then we wait for a B in the same way If the sequence fails to match the expected B at any point we return to state 0 After the start sequence has been detected we assume that we are synchronized with the GPS receiver and collect the remainder of the fixed length message Once the entire message has been collected we set a flag indicating that the main program should interpret the message The state diagram for the com input is shown in Figure 6 3 c i l i 2 i 3 Q7 i 0 c B 0 i 68 set done flag 0 Figure 6 3 Serial com state diagram for GPS data input Note that the sequence detection and message gathering must be done in the c
34. val last gps hsec unsigned long odometer total prev odometer total odometer sample long run sec run min prev sec elapsed times for output screen volatile char user key user input key T LOG DATA log data sensor input data T LOG DATA old data previous sensor input data int curr old data int old speed curr speed curr guage char old sats tracked number of satellites tracked for screen output T POS CHAN STATUS GPS chan GPS chan2 processes GPS input data unsigned char gps report 100 GPS raw input data string long gps latitude gps longitude GPS lat long from receiver long calc latitude calc longitude lat long for screen output long old calc lat old calc long old lat long for screen output unsigned long gps seconds gps hsec int old gps valid gps valid gps found several strings for output to screen char rtstr 6 speed str 5 pct str 9 tsgps str 5 char status line 41 char lat str 15 long str 15 int sen high 5 sen low 5 sen val 5 unsigned int strength 6 several numbers indicating map bounds position etc long mapullong mapullat maplrlong maplrlat int mapposx mapposy several temporary variables for various things long tlat tlong unsigned long nexttime unsigned char cursor back box 100 char map name 25 unsigned char stmp 20 long tempnum 5 double templong char gps char GPS input character
35. viewing window in latitude longitude milliseconds we can view the raw GPS message position output in real time We can see the number of satellites visible the signal strengths the GPS position etc This allows us to be immediately aware of an error such as loss of satellite fix so that we can try to find the source of the problem A screen shot of the data acquisition program Maplog is shown below in Figure 6 5 An overlay of the raw collected GPS data onto a map of Blacksburg VA is shown in Figure 6 6 61 289524984 124019805 00 3 7 ed 1215 11 10 Figure 6 6 Collected raw GPS data overlaid onto map of Blacksburg VA 62 6 3 Windows Software Development After we developed the DOS based program we desired a better way to do post processing and visualization of the data We chose the Windows 95 platform because of the native graphical user interface GUI and multi threading capabilities Also by using a programming application such as Microsoft Visual C creating an application s structure and interface is relatively simple In this chapter we will cover some of the basics of Windows 95 Win95 programming with particular emphasis on multi threading and utilizing the GUI for data visualization In addition the multimedia timer and serial communications which apply to a data collection system will be covered in detail All programming was done with Microsoft Visual C 5 0 MSVC and utilizes the Micr
36. vscreen tlat calc_latitude tlong calc_longitude if tlat lt 0 lat_str 0 tlat 0 tlat else lat str 0 2 if tlong lt 0 long_str 0 tlong 0 tlong else long str 0 2 for i 0 1 lt 9 i lat_str 9 i char tlat 10 0 long_str 9 i char tlong 10 0 tlat tlat 10 tlong tlong 10 blit_string LATSTRPOSX LATSTRPOSY lat_str TEXTCOL vscreen blit_string LONGSTRPOSX LONGSTRPOSY long_str TEXTCOL vscreen break default break get_user_key switch user_key process run time keypresses case s stop running case S running 0 break case 0 select first sensor as current curr_guage 0 draw_guage_border BORDERCOL vscreen break case 1 select second sensor as current curr_guage 1 draw_guage_border BORD break case 2 select third sensor as current curr guage 2 draw guage border BORD break sele case 3 curr guage 3 draw guage border BORD break RCOL vscreen RCOL vscreen t fourth sensor as current RCOL vscreen 98 d ta select fifth sensor as current curr guage 4 draw guage border BORDERCOL vscreen break select sixth sensor as current curr_guage 5 draw_
37. 8 R 2 2 0 00251 initialize state noise matrix Q for i 1 i lt 8 it Q i i 0 05 Q 1 1 7000 Q 2 2 7000 013113 30 014114 2 OTST TSI 0 856 Q 6 1 6 0 0463 017117 0 0533 018118 0 0304 m_CurrHeading 0 for i 0 i1 lt HEADING_FILT_SIZE i m_HeadingFilt i 0 112 initialize variables representing physical constants conversions etc m_fInitGPS FALSE m_fxkValid FALSE m_NextGPSTime 0 m_RunCount 0 old_odo_total 0 m_MvoltPerDegree 9 5 m_TicksPerMeter 27 3 m_GyroCenter 3069 0 m_SteerCenter 2919 8 m_GPSVelocityThreshold 2 8 GPSPosWeight 0 04 GPSHeadingWeight 0 6 GPSPosThreshold 3 0E8 MaxDistPer100 0 45 NumGoodNoWeight 0 odo zero count 0 m GPSNoUpdateThresh 30 m GPSNoUpdateThresh2 3000 meters to msec longzmeters to msec lat 20 filter count 20 P P P Initialize fuzzy variables GPSToCurrDist SetNumMembers 3 GPSToCurrDist SetMemberFunc SMALL 3 0 0 1 5E7 2 0E7 SMALL 3 GPSToCurrDist SetMemberFunc MED 3 1 0E7 5 0E7 1 0E8 3 5E8 MED 3 GPSToCurrDist SetMemberFunc LARGE 3 2 0E8 3 0E8 10 0E12 10 0E12 LARGE 3 GPSCurrVelocity SetNumMembers 3 GPSCurrVelocity SetMemberFunc SLOW 3 0 0 2 0 2 5 SLOW 3 GPSCurrVelocity SetMemberFunc MED 3 2 0 2 5 3 0 3 5 MED 3 GPSCurrVelocity SetMemberFunc FAST 3 2 5 3 0 500 0 500 0 FAST 3 GPSHeadWeight SetNumMe
38. 8 timer interrupt jump vector location define PIT_FREQ 0x1234DDL frequency of PIT chip void interrupt far BIOSTimerHandler void old timer ISR pointer void SetTimer void interrupt _ far TimerHandler void int frequency clock_ticks 0 initialize clock counter counter long PIT_FREQ frequency determine PIT divisor BIOSTimerHandler getvect TIMERINTR get address of old timer handler setvect TIMERINTR TimerHandler set 100 Hz timer handler jump vector outp 0x43 0x34 set PIT chip frequency outp 0x40 counter 256 output low byte of counter value outp 0x40 counter 256 output high byte of counter value void CleanUpTimer void outp 0x43 0x34 reset PIT chip frequency outputting 0x0000 to the PIT chip sets the maximum counter value of 0x10000 outp 0x40 0x00 send low byte of counter value outp 0x40 0x00 send high byte of counter value setvect TIMERINTR BIOSTimerHandler set old timer handler jump vector The following table contains a list of many of the important system interrupts that can be called as listed in Programmer s Problem Solver Jourdain 1992 55 Vector Funcion vector Funcion 00h Divide by zero error 14h Com port driver Diskette drive controller F Printer controller 12h Memory size check 26h Absolute disk sector write Disk I O PC XT 27h Te
39. A screen shot of this display window is in Figure 6 8 PostView Figure 6 8 Sensor data view of post processing application As with the DOS data collection program we would like to be able to see the latitude and longitude plotted on some sort of display graphically Additionally we 68 would like to see both the raw GPS input position and the filtered output position estimate from the system All of this data is shown in a separate window which can be resized as desired Additionally the post processing program can read in a file which defines the road segments as discussed in the map matching section of this thesis and it can display these road segments over which the vehicle position is plotted Although difficult to distinguish in printed form both the collected raw GPS data and the filtered output position are plotted simultaneously for comparison A screen capture of this window is shown in Figure 6 9 Figure 6 9 Graphical output showing raw GPS data filtered output and current map 69 Chapter 7 Results The results for this project have been very promising We have shown that with a system using only a few inexpensive inertial sensors along with a common GPS receiver we are able to produce a much better position estimate output than is output from the GPS receiver alone This is true both for areas of good GPS coverage where the inertial data augments the position estimate capabilities and for areas of ba
40. CE BETWEEN INTERSECTIONS 47 VEHICLE HEADING VERSUS DISTANCE WHEN TRAVELING STRAIGHT THROUGH AN INTERSECTION 47 VEHICLE HEADING VERSUS DISTANCE WHEN TURNING IN AN INTERSECTION MESH OF NODES INTERSECTIONS MAP REPRESENTATION VEHICLE TRAVELING THROUGH AMBIGUOUS REGION SURROUNDING AN INTERSECTION eee 49 ERROR REDUCTION USING AMBIGUOUS REGION IN MAP MATCHING ALGORITHM ccccceceeeeeeeeeeeeeeeeeeeeeeeeeeeeeees 50 BASIC BLOCK DIAGRAM OF SENSOR FUSION 2 51 BLOCK DIAGRAM OF ALGORITHM WITH ALTERNATE INPUT AND OUTPUT LOCATIONS 0 020202022 000111 52 SERIAL COM STATE DIAGRAM FOR GPS 4 4444444000000000000000000000104442 2220 57 VGA SCREEN MEMORY 2 9 272 59 MAPLOG DATA ACQUISITION PROGRAM SHOWING PLOT OF GPS 62 COLLECTED RAW GPS DATA OVERLAID ONTO MAP OF BLACKSBURG 62 GPS INFORMATION VIEW OF POST PROCESSING APPLICATION 2 68 SENSOR DATA VIEW OF POST PROCESSING 22 68 GRAPHICAL OUTPUT SHOWING RAW GPS DATA FILTERED OUTPUT AND CURRENT 69 SENSOR FUSION SYSTEM CORRECTS FOR LOSS OF GPS SIGNAL IN 4 4 000001000004 71 DETAILED CROSS SECTION VIEW OF EARTH FOR GEODETIC TO GROUND CONVERSION eee 81 DOS DATA COLLECTION PROGRAM SOFTWARE
41. FY offset color vscreen draw border around satellite signal strength indicators void draw_strength_border unsigned int color unsigned char far vscreen int i for i 0 i lt 6 i h_line GBULX STROFFX i STRSPACE GBULX STROFFX STRWIDTH i STRSPACE GBULY STROFFY color vscreen h_line GBULX STROFFX i STRSPACE GBULX STROFFX STRWIDTH i STRSPACE GBULY STROFFY STRHEIGHT color vscreen v_line GBULY STROFFY GBULY STROFFY STRHEIGHT GBULX STROFFX i STRSPACE color vscreen v line GBULY STROFFY GBULY STROFFY STRHEIGHT GBULX STROFFX i STRSPACE STRWIDTH color vscreen draw current satellite strength bar void draw_strength_vals unsigned int color unsigned char far vscreen 107 int i offset for i 0 i lt 6 i offset int float STRUPPER strength i float STRUPPER STRLOWER float STRHEIGHT 2 1 h_line GBULX STROFFX i STRSPACE 1 GBULX STROFFX STRWIDTH i STRSPACE 1 GBULY STROFFY offset color vscreen put a bitmap with width height at position sx sy void put_box int sx int sy int width int height unsigned char box unsigned char far vscreen int vsoff i j boxoff boxoff 0 vsoff sy lt lt 6 sy lt lt 8 Sx for i 0 i lt height i for j 0 j lt width j vscreen vsoff j box boxoff vsoff 320 get a bitmap with width height at position sx sy void get_box int sx int
42. GIS and IVHS Van Nostrand Reinhold New York 1995 Mazidi M and Mazidi J The 80x56 IBM PC amp Compatible Computers Assembly Language Design and Interfacing Prentice Hall Englewood Cliffs 1995 Miller Kenneth S and Donald M Leskiw Introduction to Kalman Filtering with Applications Robert E Krieger Publishing Company Malabar 1987 76 21 22 23 24 25 26 27 28 29 Miyazaki J Higashimae R Kurihara T and Ishino S Vibratory Gyroscope and Various Applications Technical Paper Murata Manufacturing Co Ltd Kyoto 1994 K Fuzzy Adaptive Stabilization of Higher Order Kalman Filters Research Report University of California at Berkeley 1996 Motorola Oncore User s Guide Motorola Inc 1996 Press W Flannery B Teukolsky S and Vetterling W Numerical Recipes in C The Art of Scientific Computing Cambridge University Press Cambridge 1988 A and Cross P Kalman Filter for an Integrated Land Vehicle Navigation System Journal of Navigation Vol 48 May 1995 Rao V and Rao H C Neural Networks amp Fuzzy Logic MIS Press New York 1995 Roden T High Resolution Timing Dr Dobb s Journal September 1992 Svensson A and Holst J Integration of Navigation Data Journal of Navigation Vol 48 May 1995 Weiss J Analysis of Upgraded GPS Internal Kalman Fil
43. GPSPosWeight is SMALL if GPSToCurrDist is SMALL and GPSCurrVelocity is SLOW then GPSPosWeight is SMALL memval MAX MIN GPSToCurrDist GetMembership MED_3 GPSCurrVelocity GetMembership MED 3 MIN GPSToCurrDist GetMembership SMALL 3 GPSCurrVelocity GetMembership SLOW 3 GPSPosWeight SetMembership SMALL 5 memval GPSHeadWeight SetMembership SMALL 5 memval if GPSToCurrDist is MED and GPSCurrVelocity is SLOW then GPSPosWeight is ZERO if GPSToCurrDist is LARGE and NumNoGood is SMALL then GPSPosWeight is ZERO if GPSToCurrDist is LARGE and NumNoGood is MED then GPSPosWeight is ZERO memval MAX3 MIN GPSToCurrDist GetMembership LARGE_3 NumNoGood GetMembership MED 3 MIN GPSToCurrDist GetMembership LARGE 3 NumNoGood GetMembership SMALL 3 MIN GPSToCurrDist GetMembership MED 3 GPSCurrVelocity GetMembership SLOW 3 GPSPosWeight SetMembership ZERO 5 memval GPSHeadWeight SetMembership ZERO 5 memval if GPSToCurrDist is LARGE and NumNoGood is LARGE then GPSPosWeight is ONE memval MIN GPSToCurrDist GetMembership LARGE 3 NumNoGood GetMembership LARGE 3 GPSPosWeight SetMembership ONE 5 memval GPSHeadWeight SetMembership ONE 5 memval get de fuzzified output weighting values m GPSHeadingWeight GPSHeadWeight GetValue m GPSPosWeight GPSPosWeight GetValue use threshold to determine if the data was fused significant
44. K DIAGRAM 25 TIGHTLY COUPLED GPS INS SYSTEM 555555115521 28 LOOSLEY COUPLED GPSANS SYSTEM eee EET ie ee o 28 BASIC SENSOR FUSION ALGORITHM 28 CROSS SECTION VIEW OF ELLIPTICAL 2 000000000000 01001 555111 29 FUSION ERROR DUE TO THRESHOLD VALUE IN RULE BASED FUSION 0 nnrennnenenereneeneees 37 FUZZY MEMBERSHIP FUNCTIONS FOR eene eene n eene enne nennen nene eene en nen nennen eene nenne nennen eene enne 37 FUZZIFICATION OF A CRISP DATA VALUE eene 38 OUTPUT MEMBERSHIP FUNCTION FOR POSITION WEIGHTING VALUE ccce eene eene nennen nennen nennen nenne nenne 39 FUZZ Y OUTPU TOF THE SYSTEM vous sea eoe e dedu sa Tet DX Ee 40 CENTROID REPRESENTING CRISP OUTPUT OF THE SYSTEM 40 ITERATIVE HILL CLIMBING OPTIMIZATION METHOD 43 ELECTRONIC GENOTYPE REPRESENTATION OF 5 000099 44 REPRODUCTION OF ELECTRONIC nennen eene 44 GENETIC CROSSOVER OF ELECTRONIC GENOTYPES 45 GENETIC MUTATION OF ELECTRONIC 5 2 nennen eene eene nennen 45 VEHICLE HEADING VERSUS DISTAN
45. Multi rate Sensor Fusion for GPS Navigation Using Kalman Filtering by David McNeil Mayhew Thesis submitted to the Faculty of the Virginia Polytechnic Institute and State University in partial fulfillment of the requirements for the degree of MASTER OF SCIENCE IN ELECTRICAL ENGINEERING Dr Pushkin Kachroo Chairman Dept of Electrical Engineering Dr John Bay Dept of Electrical Engineering Dr Joseph Ball Dept of Mathematics May 1999 Blacksburg Virginia Multi rate Sensor Fusion for GPS Navigation Using Kalman Filtering by David McNeil Mayhew Committee chairman Dr Pushkin Kachroo Bradley Department of Electrical Engineering ABSTRACT With the advent of the Global Position System GPS we now have the ability to determine absolute position anywhere on the globe Although GPS systems work well in open environments with no overhead obstructions they are subject to large unavoidable errors when the reception from some of the satellites is blocked This occurs frequently in urban environments such as downtown New York City GPS systems require at least four satellites visible to maintain a good position fix Tall buildings and tunnels often block several if not all of the satellites Additionally due to Selective Availability SA where small amounts of error are intentionally introduced GPS errors can typically range up to 100 ft or more This thesis proposes several methods for improving the positio
46. S problems We assume that the filtering algorithm will correct this in a short amount of time Otherwise if GPS is good the GPS position is close to the current position estimate and the GPS velocity is over a specified minimum then we do a weighted sum of the current estimate and the GPS position and heading In general we want this to be the rule that is always executed indicating that the current estimate and the GPS positions agree The GPS velocity is required to have a 35 minimum value because the position output from the GPS receiver tends to drift backwards when the vehicle is not moving If GPS has been good for an amount of time but the GPS position has been too far from our estimated position to fuse the data we extend the position threshold a small amount and check again This rule is to cover the unlikely event that our estimated position has been thrown off for some reason Although it is not very likely it is possible for our estimate to get out of position just far enough so that the estimate and the GPS positions are separated by a distance too great to consider valid In this case we detect that the GPS data has been good for a significant amount of time so we assume that our estimate is off and converge our estimate to the GPS position If GPS has been good for a very long time but the GPS position has drifted much too far from the estimate position to fuse in either of the above cases we reinitialize the
47. XHEIGHT cursor back box vscreen blit bit mapposx mapposy RED vscreen recalc mapposx mapposy if calc_longitude gt mapullong amp amp calc_longitude lt maplrlong amp amp calc_latitude lt mapullat amp amp calc_latitude gt maplrlat POINT pl p2 pl x calc_longitude pl y calc_latitude 2 11 pl mapposx int p2 x mapposy int p2 y 1 mapposx mapposy 0 if mapposx mapposy get box at mapposx mapposy get box mapposx 2 mapposy 2 CURSBACKBOXWIDTH CURSBACKBOXHEIGHT cursor back box vscreen draw marker v line mapposy 2 mapposy 2 mapposx MAPPOSCOL vscreen h line mapposx 2 mapposx 2 mapposy MAPPOSCOL vscreen switch int hsec 10 case 2 time 2 seconds gt update runtime strings run_sec hsec start_hsec 6000 100 run min hsec start hsec 3600001 6000 if run sec prev sec output new time to screen if prev sec 1 blit string RTPOSX RTPOSY rtstr BACKGNDCOL vscreen rtstr 0 char run_min 100 10 0 rtstr 1 char run min 10 0 rtstr 3 char run_sec 100 10 0 rtstr 4 char run sec 10 0 blit string RTPOSX RTPOSY rtstr TEXTCOL vscreen prev_sec run_sec break case 4 time xxxxx 4 seconds gt update current time string if logging pct val hsec else pct_val log_data ticks if old_pct_val 100 pct_val 100 new second t
48. ads and segments map roads farmalloc 24 num map roads map segments farmalloc sizeof ROAD SEG num segments if map roads NULL map segments NULL printf Error allocating memory n prinpf Excting Ge NEA goodbye 0 map roads and segments for i 0 i lt num_map_roads itt fread amp map roads i 24 24 1 map file for i 0 i lt num map_segments i fread amp map segments i sizeof ROAD SEG 1 map file fclose map file map file NULL mapping 1 else if strcmp argv il novga strcomp argv i NOVGA novga 1 disable VGA graphics output else if strcmp argv il c char cfgfilename 20 char var 20 znt 3f itt strcpy cfgfilename argv il strcat cfgfilename cfg open configuration input file if cfg file fopen cfgfilename r NULL printf Error opening configuration file s for input n cfgfilename printf Exiting c goodbye 0 read configuration parameters while fscanf cfg file s var EOF for j 0 j lt strlen var j var j 0x20 make lowercase if stremp var mapullong fscanf cfg file Slu amp mapullong else if strcmp var mapullat fscanf cfg_file lu amp mapullat else if strcmp var maplrlong fscanf cfg file lu amp maplrlong else if strcmp var maplrlat fscanf cfg file lu amp maplrlat else
49. al state estimate of a system given a set of measurements and relationships between the measurements and the state vector It be applied when the relationship between the state vector and the measurement vector can be written as X k 1 A k X k w k Y k CX k v k where X k is the state vector at time step k and Y k is the measurement vector at time step k A k is the state transfer matrix and C is the observation matrix w and v are zero mean white gaussian noise variables In our project we use an eight state Kalman filter The two values in the measurement vector are velocity from the ABS wheel encoder and angular velocity from a gyroscope The state vector X and the measurement vector Y are shown here x msec deg bed Sec meters ao ter 31 where x and y are the longitude and latitude position estimates respectively is the current vehicle heading estimate and AO is the current vehicle velocity estimate O stands for odometer so represents a change in odometer value distance which is velocity We are using two measurement values velocity based on a wheel encoder output AO and angular velocity based on a gyroscope output 0 The encoder tick meters per second conversion factor is a parameter which may be affected by tire pressure sensor error etc The gyroscope output angular velocity conversion requires both a center point the sensor outp
50. arameter optimization are presented along with a novel map matching algorithm Chapter 6 covers the implementation of the entire application in software This covers details regarding development in both a DOS and Windows 95 programming environment under C This chapter gets into the specifics of programming a real time application under DOS such as interrupt driven communications and timing as well as some Windows graphical user interface GUI design considerations Chapter 7 presents the results of the project based on test runs in Blacksburg Virginia and New York New York We present conclusions of this project in Chapter 8 along with potential avenues of continued research Chapter 2 Literature Survey Before the main topic of this thesis is presented we first present the reader with a brief summary of information that we collected from various sources including books conference papers and other theses Presented first is information relating to systems and algorithms using INS only Then we summarize work relating to GPS INS sensor fusion in particular Lastly we present several developed systems that perform GPS INS fusion 2 1 Inertial Navigation Billur Barshan and Hugh F Durrant Whyte 1995 utilize a system consisting of three gyroscopes a tri axial accelerometer and two tilt sensors to perform inertial navigation They focus on careful and detailed error modeling to obtain a position drift rate of 1 8 cm s dep
51. articular measurement variable the corresponding value in the measurement noise matrix R would be very low Conversely if a very noisy instrument were being used the corresponding value in R would be very high In our particular application of the Kalman filter we used the following matrices for the state transfer matrix A k and the observation matrix C 100 0 0 cokk 0 0 010 0 0 sin k O 0 2 0 01 A 7 0 0 0 420 Ot 0 0 0 0000 1 0 o 0 0000 0 1 10 000 0 0 0 1 At 000 0 0 0 0 1 000 1 0 0 0 0 000 0 0 1 0 0 Based on our collected data we were able to determine appropriate values for the measurement noise matrix R and the state noise matrix The values in were obtained by finding the variance in the measurement data that was collected Likewise the values in Q were obtained by finding the variance in the state variable data that was collected and processed We have assumed that the state variables and measurement variables are uncorrelated which is why there are only diagonal terms in R and 2 618 0 0 0 0251 33 7000 0 0 0 0 0 0 700 0 0 O 0 0 0 0 0 300 0 0 0 0 0 0 02 0 0 0 0 9 0 0 0 0 0856 0 0 0 0 0 0 00463 0 0 0 0 0 0 0 0533 0 0 0 00 O 0 0 0 0304 Before the recursive Kalman filter can operate we first needed to initialize it appropriately This consisted of assigning an initial state vector 0 and an initial state covariance matrix 0 The covariance matrix is in
52. as a real organism s characteristics are encoded in DNA the pseudo organisms characteristics are encoded in an electronic genotype which mimics the DNA of natural life The electronic genotype is merely a string of bits that represents a given sequence of sensor fusion parameters Entire we Figure 5 12 Electronic genotype representation of parameters Just as reproduction genetic crossover and mutation alter a natural DNA sequence similar operations can be used to alter the electronic genotypes Additionally the production of new genotypes and the elimination of unfit genotypes can be used to modify the set of genotypes Reproduction is the process of producing one child genotype as a result of merging the characteristics of two parent genotypes Parent Genotype 1 Parent Gekotype 2 cn Gem Figure 5 13 Reproduction of electronic genotypes 44 Genetic crossover is accomplished by crossing the genes of two genotypes replacing the original two with the altered versions Before compe fermens e After Figure 5 14 Genetic crossover of electronic genotypes Mutation simply involves replacing a genotype with a slightly altered version of itself This process induces small variations in a random manner for the purpose of finding a better parameter set Parameter
53. at messages refer to Appendix C When processing the sensor data we would like to know whether the GPS data being returned is likely to be good or not A simply method of determining this is based on the status information returned from the 12 receiver with each data message The GPS receiver returns a status byte with the following information Bit 7 Position propagate mode Bit 6 Poor geometry DOP gt 20 Bit bi fix Bit 4 Altitude hold 2D fix Bit 3 Acquiring satellites position hold Bit 2 Differential Bit 1 Insufficient visible satellites lt 3 Bit 0 Bad almanac A given GPS sample is considered bad if any of bits 0 1 or 6 is set or if either of bits 4 and 5 is not set That is the almanac data information about the location of the GPS satellites in space must be accurate and a sufficient number of satellites must be visible 3 or more Also the receiver must have both a 2D and a 3D fix location estimate to be considered good Checking the status byte does not guarantee that the data returned from the receiver is accurate but it does quickly eliminate many bad GPS data points from consideration The gyroscopes accelerometers DaqBook and GPS receiver all are mounted securely on a single piece of aluminum in the trunk of the test vehicle shown in Figure 3 9 Video cassette 12 volt DC to 115 volt AC c S inverter n F EN Data _
54. at mult Ak Pkm1 mtemp1 8 8 8 mat mult transpose mtempl Ak mtemp2 8 8 8 mat add mtemp2 Q Pk1 8 8 find Kalman gain based on predicted error covariance Kk Pkl CT C PKI OT R 1 mat mult C Pk1 mtemp3 2 8 8 mat mult transpose mtemp3 C mtemp4 2 8 2 mat add mtemp4 R mtemp5 2 2 mat inverse mtemp5 mtemp6 2 mat mult transpose Pk1 C mtemp7 8 8 2 mat mult mtemp7 mtemp6 Kk 8 2 2 gt estimate state based on old state and difference between predicted observations and actual observations Ld xk Ak xkml Kk yk C Ak xkml mat mult C Ak mtemp8 2 8 8 mat mult vector mtemp8 xkml mtemp9 2 8 vec sub yk mtemp9 mtemp10 2 mat mult vector Kk mtemp10 mtemp11 8 2 mat mult vector Ak xkml1 mtemp12 8 8 vec add mtemp12 mtempll xk 8 get error covariance Pk Kk C Pk1 118 mat mult Kk C mtemp13 8 2 8 mat mult mtemp13 Pk1 mtemp14 8 8 8 mat_sub Pk1 mtemp14 Pk 8 8 gt current state values are in xk 4 longitude xk 1 and latitude xk 2 if m DebugOut print a bunch of stuff to a file for debug puposes char str 150 int i for 1 0 1 lt 8 1 sprintf str 51f xk i l if m_DebugFile NULL m DebugFile Write str strlen str sprintf str n if m_DebugFile NULL m_DebugFile gt Write str strlen str gt update for next time 1 xk Pkml Pk mat co
55. avorite for inertial navigation systems Gyroscopes like accelerometers require no signal attachments directly to the vehicle Unlike accelerometers however gyroscopes always output a signal proportional to angular velocity independent of the speed of the vehicle In the case of using an accelerometer speed was needed to determine angular velocity This meant that any error in the speed measurement would be magnified in the overall position estimate This problem does not occur when using gyroscopes Some problems are apparent with gyroscopes like other INS devices Changes in temperature or humidity can cause the operating parameters of a gyroscope to change slightly which will introduce heading and position errors over time This can be corrected somewhat by use of adaptive parameter estimation which will be covered in the chapter on advanced algorithm development 4 2 6 Steering Position Steering position information is useful because it provides us with a direct measurement of a physical condition of the vehicle We assume that a measurement of steering shaft position is directly proportional to the front tire angle of the vehicle This in turn provides a way to calculate angular velocity given the speed of the vehicle As with accelerometers because the angular velocity calculation depends on vehicle speed errors in the speed measurement are magnified in the final position estimate There are several other drawbacks to using a steeri
56. be on a road between intersections or near an intersection when no turn is detected or near an intersection when a turn is detected 46 Heading Distance Figure 5 16 Vehicle heading versus distance between intersections 4 Heading Distance Figure 5 17 Vehicle heading versus distance when traveling straight through an intersection Turn Detected Distance Figure 5 18 Vehicle heading versus distance when turning in an intersection 47 To allow for efficient traversal of the roadways the map must be stored in particular format in memory The format developed for this project uses a linked mesh of nodes in memory Each node represents a specific intersection and contains latitude and longitude information about the intersection Additionally each node points to every adjacent node to which there is a connecting roadway Each node can be represented in computer memory by the following structure typedef struct _Intersection int NumConnections number of adjacent intersections _Intersection Connections pointers to adjacent intersections int LatitudeMsec LongitudeMsec lat long of this intersection Intersection Where LatitudeMsec and LongitudeMsec represent the position of the intersection in milliseconds 1 msec longitude equals approximately 1 15 inches at the equator Connections points to a list of pointers to adjacent nodes of length NumConnections Upon initialization of the map st
57. bers L ed Practical Handbook of Genetic Algorithms Volume I CRC Press New York 1995 7 Da Ren and Ching Fang Lin Failure Diagnosis Using the State Chi Square Test and the ARTMAP Neural Networks Proceedings of the American Control Conference June 1995 8 DaqBook User s Manual Rev 3 IOtech Inc April 1994 9 Escobal Pedro Ramon Methods of Orbit Determination John Wiley amp Sons Inc New York 1965 75 10 11 12 13 14 15 16 17 18 19 20 Gyrostar Piezoelectric Vibrating Gyroscope Technical Paper Murata Manufacturing Co Ltd Kyoto 1994 Hofmann Wellenhof B Lichtenegger H and Collins J GPS Theory and Practice Springer Verlag Wien New York 1997 Horst R Pardalos P and Thoai N Introduction to Global Optimization Kluwer Academic Publishers Boston 1995 Jang J S R Sun C T and Mizutani E Neuro Fuzzy and Soft Computing Prentice Hall Inc Upper Saddle River 1997 Jourdain R Programmer s Problem Solver Brady Publishing New York 1992 Kaplan Elliot D ed Understanding GPS Principles and Applications Artech House Publishers Boston 1996 LaMothe A Ratcliff J Seminatore M and Tyler D Tricks of the Game Programming Gurus Sams Publishing Indianapolis 1994 Logsdon T The Navstar Global Positioning System Van Nostrand Reinhold New York 1992 Logsdon T Understanding the Navstar GPS
58. c in state matrix double newhead 90 0 m_CurrGPSData heading newhead fmod newhead 360 0 360 0 xkm1 1 m_GPSLongitude xkm1 2 m GPSLatitude xkm1 3 newhead calculate local meters gt msec lat long here first convert latitude msec to radians double tlat DEG TO RAD m GPSLatitude 3600000 0 double tsin sin tlat meters to msec long pow 1 E2 tsin tsin 0 5 0 03083070 cos t1at meters to msec lat pow 1 E2 tsin tsin 1 5 0 030715169 m LastGoodGPSLat xkm1 2 m LastGoodGPSLong xkm1 1 m LastGoodGPSDLat 0 m LastGoodGPSDLong 0 else if m_GPSGood amp amp m_FuseData fuzzy rules if GPSToCurrDist is SMALL and GPSCurrVelocity is FAST then GPSPosWeight is LARGE if GPSToCurrDist is MED and GPSCurrVelocity is FAST then GPSPosWeight is MED if GPSToCurrDist is SMALL and GPSCurrVelocity is MED then GPSPosWeight is MED if GPSToCurrDist is MED and GPSCurrVelocity is MED then GPSPosWeight is SMALL if GPSToCurrDist is SMALL and GPSCurrVelocity is SLOW then GPSPosWeight is SMALL if GPSToCurrDist is MED and GPSCurrVelocity is SLOW then GPSPosWeight is ZERO if GPSToCurrDist is LARGE and NumNoGood is SMALL then GPSPosWeight is ZERO if GPSToCurrDist is LARGE and NumNoGood is MED then GPSPosWeight is ZERO if GPSToCurrDist is LARGE and NumNoGood is LARGE then GPSPosWeight is ONE
59. choice 4 2 4 Tilt Sensors Tilt sensors are used to measure the pitch or roll of a vehicle Pitch and roll data may or may not be useful depending on the intended application For example if we are concerned that the vehicle may traverse severe inclines while driving then we may need 18 to use tilt information to compensate for accelerometer errors Suppose we have one of the following exaggerated cases 1 4 Figure 4 4a Vehicle with side tilt Figure 4 4b Vehicle with front tilt In Figure 4 4a the vehicle is shown with a severe side tilt due to a tilted road surface In this case an onboard accelerometer will read a significant side to side acceleration indicating that the vehicle is continuously turning in one direction Similarly in Figure 4 4b the vehicle is shown with a forward tilt due to a sloped road surface In such a situation an accelerometer would read a continuous acceleration as if the vehicle were continually decelerating Figure 4 5 A typical liquid filled tilt sensor In either case the accelerometer readings are misleading about the vehicle s motion because of an inclined road surface An additional sensor such as a tilt sensor can be used to indicate if a given force is due to acceleration of the vehicle in a particular direction or if it is due to a tilt in the vehicle body 19 4 2 5 Gyroscopes Gyroscopes have advanced significantly the last several years and a f
60. conds m_CurrGPSData latitude minutes 60 0 abs m_CurrGPSData latitude degrees 3600 0 1000 0 SGN m_CurrGPSData latitude degrees m GPSLongitude int m_CurrGPSData longitude seconds m CurrGPSData longitude minutes 60 0 abs m CurrGPSData longitude degrees 3600 0 1000 0 SGN m CurrGPSData longitude degrees if m CurrGPSData rcvr status amp 0x43 m CurrGPSData rcvr status amp 0x30 m GPSGood FALSE else m GPSGood TRUE update GPS page w new current GPS data m pGPSPage m GoodGPSCheckVar m GPSGood m pMapDlg PlotRawGPS CPoint m GPSLongitude m GPSLatitude m GPSGood update output window m pGPSPage UpdateContents FilterlHz for GPS data if m Fuzzy FilterlHz use rule based fusion else FuzzylHz use fuzzy fusion read next GPS data into m NextGPSData if m pGPSFile Read amp m NextGPSTime sizeof unsigned int sizeof unsigned int m pMainPage SetRunState FALSE return TRUE m_pGPSFile gt Read amp m_NextGPSData sizeof T_POS_CHAN_STATUS sleep to effect a pseudo accurate timing if m pMainPage m HertzEditVar Sleep 1000 m pMainPage m HertzEditVar else Sleep 100 keep from bogging down system in tight loop return TRUE return TRUE so that OnIdle is called again 111 void CPostViewApp InitFilter void intesi i do anything related to initializing the filter here just to distinguish filter re
61. d fully in many navigation systems However with today s portable computers and complete maps on a single compact disc map matching can be used more for real time position estimation purposes Chapter 3 Hardware Description The hardware chosen for this project consists of several main components These are the test vehicle itself inertial and dead reckoning sensors a data acquisition board a GPS receiver and a laptop computer All of these components are fairly common and inexpensive Each of these hardware sub systems will be covered in detail In addition some of the software issues specific to the hardware will be discussed 3 1 Test Vehicle The test vehicle for our experiments was a 1997 Ford Taurus 4 door sedan No special equipment was installed on the vehicle before the research During the course of our work however several items were added to aid in data collection We mounted a small black and white charge coupled device CCD camera behind the rear view mirror such that it could view the road and surroundings directly ahead but would not distract the driver In addition we placed a small microphone on the underside of the sun visor with a switch on the upper left portion of the dash so that the driver could record his voice when desired The outputs of both of these devices were fed under trim pieces of the vehicle and into the trunk where they input into a VHS videocassette recorder Also we added a simple 12 volt
62. d or intermittent GPS coverage where the inertial data serves as the only means of position estimation for a period of time For this project the improvement that was shown was primarily qualitative That is due to the difficult nature of doing so precise methods of quantifying relative performance were not developed However the intent of this project was to show that integrated inertial and GPS data can produce a significantly better system output than GPS alone Some methods for precisely evaluating the system output performance are presented in the following chapter and methods for fine tuning system parameters were presented in the chapter on algorithm extensions As shown in Figure 7 1 the system has been demonstrated to be able to compensate for complete and partial GPS losses for over 1 full city block The worst problems arose when the GPS signal was considered to be good by our simple validity test when it was actually significantly off the correct position This could happen as a result of multi path errors or some other error that is not easily detectable This problem can be addressed by either improving the validity test for the GPS data or by fine tuning the sensor fusion parameters 70 Loix Zoom Pan Folow 286246033 146714412 266252867 146878149 eg T Tunnel results in GPS blockage if ee y invalid or no GPS output for E Figure 7 1 S
63. d programming with the various considerations for each type of programming 6 1 Software Structure and Data Flow The general data flow in the software should closely resemble the flow in the filtering algorithm Therefore if we are filtering at both 100 Hz and 1 Hz our software should reflect this in its design The core of the filtering algorithm flow is shown here Inertial data collection at 100 Hz Inertial sensor Sensor fusion at System output 1 Hz GPS data GPS Sensor collection at 1 input Hz Figure 6 1 Basic block diagram of sensor fusion algorithm We desire some more functionality in the software than simple collecting and processing on line For example we desire the ability to dump all of the collected data to the PC s hard drive for later analysis Similarly we desire the ability to read the collected data from the hard drive and run our program just as if the collection were happening in real time In addition we want several methods of viewing the output from the software such as real time viewing of the raw GPS data as well as the filtered position data for comparison These additions complicate our software design as shown in Figure 6 1 51 Inertial sensor Inertial data Sensor fusion at Output collection at 1 Hz 100 Hz Hard Drive 9 GPS data collection at 1 GPS sensor Hz input Figure 6 2 Block diagram of algorithm with alternate input and output locations Althou
64. dGPSLat xkm1 2 m LastGoodGPSLong xkm1 1 m LastGoodGPSDLat 0 m LastGoodGPSDLong 0 else if m_GPSGood amp amp m FuseData double dl m GPSLongitude xkm1 1 double d2 m GPSLatitude xkml 2 SQR d1 SQR d2 lt m_GPSPosThreshold if m_CurrGPSData velocity gt m_GPSVelocityThreshold perform median filtering on the GPS heading outputs to eliminate glitches m_HeadingFilt m_CurrHeading m_CurrGPSData heading 114 m CurrHeading m_CurrHeading 1 SHEADING_FILT_SIZE double SortHeading HEADING FILT SIZE for int i 0 i lt HEADING_FILT_SIZE i SortHeading i m HeadingFilt i qsort SortHeading HEADING_FILT_SIZE sizeof double dcomp m GPSHeading SortHeading HEADING FILT SIZE 1 2 weighted sum of GPS position and current position using resultant GPSPosWeight 1 1 GPSPosWeight m GPSLongitude 1 0 m GPSPosWeight xkm1 1 xkm1 2 m_GPSPosWeight m_GPSLatitude 1 0 m_GPSPosWeight xkm1 2 double newhead assumed heading and GPS sensor heading are 90 degrees out of phase and inverted double gpshead 90 0 m GPSHeading while xkm1 3 gpshead 180 0 gpshead 360 0 while gpshead xkm1 3 2180 0 gpshead 360 0 weighted sum of GPS heading and current heading using resultant GPSHeadingWeight newhead m GPSHeadingWeight gpshead 1 m GPSHeadingWeight xkm1 3 xkm1 3 Il m LastGoodGPSLat
65. drature encoder A small drawback to using some encoders is the inability to determine direction from the output Many encoders however have two sensors that output signals 90 out of phase from each other These output signals could be used to determine both speed and direction These types of encoders are known as quadrature encoders Quadrature encoders can be used to determine speed and direction from the two sensor outputs by examining the relative phase of each of the outputs 16 Direction 1 Rising edge of channel Direction 2 Rising edge of channel A leads rising edge of channel B B leads rising edge of channel A Figure 4 3 Obtaining direction from quadrature encoder outputs As was noted in the chapter on hardware the signal output reliability can affect the overall acceptability of the encoder as a distance measured In the case of this project the signal running to the odometer was initially used as a distance encoder substitute This proved unsuitable because of the unreliability of the signal at low speeds We then switched to using the output from the anti lock braking system which produces a digital signal like that of a normal encoder non quadrature We could therefore determine speed of travel with a high amount of accuracy 4 2 2 Tachometers Tachometers differ from encoders in that they provide a direct measure of speed instead of distance A tachometer typically outputs a voltage level that is proportional
66. duces the possible space that a vehicle could occupy assuming that the vehicle is actually on or near a road Additionally by knowing the exact locations of intersections we can determine if a turn detected by GPS or INS sensors is legal Simply stated we use knowledge of a region s roads to confine the position and motion of our vehicle There are only a few situations that need to be handled by our algorithm These situations are startup initialization position between intersections Figure 5 16 position near intersection and no turn detected Figure 5 17 position near intersection and turn detected Figure 5 18 and error re initialization The two cases where an initialization is required which should be very rare are the only cases where a search to find the nearest road is required In these cases our algorithm needs to determine what our starting point is This generally involves an exhaustive search of every road in the database in order to find the road of minimum perpendicular distance to our current position There are some shortcuts that can be used but this is still a relatively time consuming process In general we want our map matching algorithm to operate without doing any time consuming processing Performing map matching after a valid starting position is known does not need to involve a search of all possible roads We only have to handle three situations if a valid position is already known the vehicle could
67. e whenever a window is dragged across the screen the thread that controls that 63 window blocks until the window is released Of course the thread does not halt execution it is instead busily executing the code responsible for dragging the window However this means that the thread is not able to do anything else while the window is being moved In many applications such as a simple word processor this is not a problem the program only does anything in response to a user action anyway If we are doing real time I O or processing however we do not want our processing thread to halt when the user does something Hence we separate the tasks into the user interface portion and the data collecting and processing portions Thus when the user manipulates the user interface the processing thread can continue as if it had complete control of the CPU 6 3 2 Multimedia Timer and Real time I O There are two methods of generating timed function calls in Win95 One method involves using a simple message handler provided by the The CWnd class provides a function called OnTimer which can be used by the programmer to generate function calls at fixed intervals Many MFC classes inherit the CWnd class including windows dialog boxes buttons and many more This makes it very easy and convenient to make use of this timing capability However after working with this timing method for quite a while we found that it when set to run
68. e number of meters east corresponding to a change of 1 degree of longitude is calculated as 2n a cos bs 1 360 3600 1 5 2 a is the equatorial radius of the earth 6 378 150 meters is the eccentricity of the elliptical cross section of the earth 0 08181333 is the geodetic latitude of the observer the vehicle To summarize assume that the vehicle is on the surface of the Earth at latitude 0 Then we have the following two conversion rates to convert meters to milliseconds of latitude and longitude and vice versa 1 1 millisecond latitude meters 32 55720 1 0 0066934sin 0 gt 1 millisecond longitude cont i meters 33 3392843 1 0 0066934 sin y 30 5 3 Kalman filter for INS We use a Kalman filter in this project only to determine the relative position output from the inertial sensors The internal details of the Kalman filter are not the focus of this thesis For Kalman filtering background we refer the reader to An Introduction to Kalman Filtering with Applications Miller and Leskiw 1987 We primarily interested in the parameters that affect the performance of the filter Sensor inputs are typically converted into meaningful values such as meters degrees sec etc The parameters that are used for these conversions are subject to errors and require correction for effective filtering operation A Kalman filter can be used to produce the optim
69. e posnt pos chan channel i mode Status Message message posnt t pos chan channel i strength Status Message message posntt pos chan channel i flags Status Message message posnt t pos chan rcvr status Status Message message posn t draw the border lines around the main map view void draw map border unsigned int color unsigned char far vscreen h line MBULX MBLRX MBULY color vscreen h line MBULX MBLRX MBLRY color vscreen v line MBULY MBLRY MBULX color vscreen v line MBULY MBLRY MBLRX color vscreen draw the border lines around the sensor boxes void draw_sensor_border unsigned int color unsigned char far vscreen h_line SBULX SBLRX SBULY color vscreen h_line SBULX SBLRX SBLRY color vscreen v_line SBULY SBLRY SBULX color vscreen v line SBULY SBLRY SBLRX color vscreen 106 draw the border lines around the GPS info region void draw gps border unsigned int color unsigned char far vscreen line GBULX GBLRX GBULY color vscreen h line GBULX GBLRX GBLRY color vscreen v line GBULY GBLRY GBULX color vscreen v line GBULY GBLRY GBLRX color vscreen draw the border around each of the individual sensor boxes void draw_guage_border unsigned int color unsigned char far vscreen int i tcol ticol if color BACKGNDCOL ticol BACKGNDCOL else ticol BROWN for 1 0 1 lt 5 if i
70. e the serial port is a sort of file A very simple explanation of the process is as follows When properly set up Windows 95 maintains input and output serial buffers From the application programmer s point of view writing to the serial port is easy As long as we do not overrun the output buffer we can send any amount of data to the port abstracted as a file write process at any point in the program Serial input however occurs asynchronously to the program That is data is received at unknown times and at unknown rates In Win95 unlike DOS the program does not need to be alerted every time a byte is received on the serial port Low level handlers in the operating system fill the input buffers with a limited amount of data automatically Our program is then alerted at some point and we read some or all of the data from the input buffer abstracted as a file read Because an unknown amount of data is arriving and our program may have to wait an indeterminate amount of time for a read or write operation we make use of a mode of I O known as overlapped In this mode operations that take a long time will return with a specific error code indicating that the operation is incomplete If our program wants to read data from the com port we use overlapped I O rather than waiting for that amount of data to arrive at the port This allows the program to continue executing while an I O operation is completing Implementing overlapped I O is ver
71. e the space segment the user segment and the control segment The space segment consists of 21 satellites plus 3 active on orbit Spares arranged in six 55 degree orbit planes 10 898 nautical miles above the earth The user segment consists of the hundreds of thousands of Navstar receivers located on the ground in the air and aboard ships together with a few aboard orbiting satellites The user segment is completely passive that is the Navstar receivers only detect the signals emitted from the space segment Because the Navstar satellites tend to lose track of their precise position and the exact time a computer driven control segment is required to correct for this subtle drift The control segment consists of a group of unmanned monitor stations that track each Navstar satellite calculating the precise position and timing errors for the satellite which transmit to the satellites for error corrections Logsdon 1995 79 A 4 Differential GPS Differential positioning with GPS abbreviated DGPS is a technique for improving GPS performance where two or more receivers are used One receiver usually at rest is located at the reference site A with known coordinates and the remote receiver B is usually roving The reference or base station calculates pseudorange corrections PRC and range rate corrections RRC which are transmitted to the remote receiver in near real time The remote receiver applies the corrections to the measured pseudorang
72. early stopped based on GPS or INS velocity we completely ignore the GPS heading value that is we set equal to zero Another simple rule is this If the GPS data is bad too far from current position too few satellites bad fix etc then we set and Yaran equal to 0 effectively running on inertial navigation only However we must allow for the unlikely event that we have made an error such that we think the GPS data is bad when it is actually good Thus If GPS has had a good satellite fix for a long time but we have assumed it to be bad we must have made a mistake so we set and Pip equal to 1 The phrase long time refers to an amount of time that must be determined by experimentation with the system Obviously these rules may vary depending on the internal operations of a particular GPS receiver and parameters must be altered appropriately A brief summary of the rules which we found useful and why they were included is shown below The phrases GPS Good and GPS Bad indicate only whether the GPS data should be considered good or bad based on the status information from the receiver itself This depends on number of satellites in view signal strength etc If GPS is good and we have not initialized our position estimate then initialize the estimated position and heading equal to the GPS position and heading Note that this might result in an initial position that is not very accurate due to GP
73. egment the vehicle is currently moving The value for is determined by the expected drift in the position estimate between the most distant adjacent nodes in the given map For example if the expected drift in position estimate is 4 of the distance traveled and the most distant adjacent nodes in the map are 1 miles apart than the ambiguous region should have a radius slightly larger than 4 of 1 miles or 004 miles roughly 21 feet Ambiguous region radius ra Figure 5 20 Vehicle traveling through ambiguous region surrounding an intersection 49 When the vehicle moves into the imaginary circle surrounding an intersection the algorithm does not know on which road segment the vehicle is moving but rather it is able to estimate a probability that the vehicle is on any given road segment The probability that the vehicle is on a given road segment can be determined based on how close the vehicle is to the road segment relative to how close it is to every other road segment If the current position of the vehicle is requested then the algorithm would return the position of the vehicle projected onto the road segment with the highest probability However a position estimate for the vehicle is always maintained independent of the road segments while in the ambiguous region Upon exiting the region the algorithm would then consider the vehicle to be on the segment with the highest probability at that time instant Suppose the v
74. ehicle traveled along the path indicated by the heavy dashed line in Figure 5 21 The position estimate of the vehicle as determined without using map matching is indicated by the dotted line Notice that near intersection A the position estimate drifts towards intersection D while the vehicle actually moves on to intersection B This drift may be due to GPS blockage sensor errors etc In this case the map matched position estimate would be correctly projected along the line segment from C to A Near intersection A the position estimate would briefly snap to the road segment joining intersections A and D However the algorithm would correct this mistake as the vehicle moves away from node A and the position estimate would then correctly snap to the road segment connecting intesections A and B Ra Actual vehicle motion path Position estimate independent of map matching Figure 5 21 Error reduction using ambiguous region in map matching algorithm 50 Chapter 6 Software Design This chapter presents the software development for the sensor fusion project Without proceeding through all of the intermediate problems and corrections that occurred throughout the development cycle we will demonstrate how the data was collected and processed and why the code was written the way it was First we will present the overall structure and data flow of the software Then we will cover both DOS based and Windows base
75. eiver as taken from the Motorola Oncore User s Guide 1996 To set the response message rate of the GPS receiver the following message is sent from the user to the GPS receiver BamC lt CR gt lt LF gt m mode 0 output response message once polled 1 255 response message output at indicated rate continuous once per second 2 once every two seconds 255 once every 255 seconds C checksum The following response would be sent from the GPS receiver to the user at the specified rate 0Bamdyyhmsffffaaaaoooohhhhmmmmvvhhddtntims dimsdimsdimsdimsdimsdsC lt CR gt lt LF gt Where we can interpret is as follows Date m month 1 12 d day 1 31 yy year 1980 2079 Time h hours 0 23 m minutes 0 59 s seconds 0 60 ffff fractional seconds 0 999 999 999 0 0 to 0 999999999 Position aaaa latitude in 324 000 000 324 000 000 90 to 90 longitude in msec 648 000 000 648 000 000 180 to 4180 hhhh height in cm 100 000 1 800 000 GPS ref ellipsoid 1000 00 to 18 000 00 meters mmmm height cm 100 000 1 800 000 MSL ref 1000 00 to 18 000 00 meters Velocity vv velocity in cm sec 0 51400 0 to 514 00 m sec hh heading 0 3599 0 0 to 359 9 deg 87 Geometry dd current DOP 0 999 0 0 to 99 9 DOP DOP type Satellite visibility and tracking status n
76. ending on the frequency of acceleration changes This like any system requires additional information from some absolute position sensing mechanism to overcome long term errors However they show that a low cost inertial sensing system can be used to provide valuable orientation and position information particularly for outdoor mobile robot applications A Svensson and J Holst 1995 have simulated a variety of filter configurations for the purpose of submarine navigation based on several inertial sensors They had the most success with a complex fourteen state Extended Kalman Filter which used eight states to describe the motion of the submarine and six to describe the measurement system Kirill Mostov 1996 used a hybrid least mean squares LMS Kalman filter for the purpose of maintaining stability in a system where inaccuracies in the model would otherwise cause instability This was done by using the Kalman filter to remove noise from the system and using the LMS to compute the weight functions which could be translated into the Kalman gain values in an iterative fashion 2 2 GPS INS Fusion Allison Ramjattan and Paul A Cross 1995 use only a gyroscope and an odometer encoder along with a GPS receiver to produce a fused output They have used this system in the streets on central London and have demonstrated the improved position estimate obtained from fusing data from only a few sensor inputs They evaluated the effect
77. ensor fusion system corrects for loss of GPS signal in tunnel Figure 7 1 shows one instance where the sensor fusion algorithm worked very well to compensate for a total GPS signal loss The white line on the upper left image indicates that path that was taken during one data collection run The upper right image is a screenshot of the map output view of the post processing application which shows where the GPS signal is completely lost and the sensor fusion algorithm switches to dead reckoning using only inertial data Note that this is difficult to see in printed form because the colors become indistinguishable In addition when the GPS signal is reestablished the current position estimate and the GPS output data are integrated in a smooth transition The lower left and right images are snapshots from the onboard VCR while entering the tunnel and inside the tunnel respectively 71 Chapter 8 Conclusions Further Research 8 1 Further Research The results from this project are encouraging and show that these techniques do work to improve position estimates in urban environments However there is much room for improvement in the overall techniques as well as the particular implementation discussed Some of these suggested improvements will be discussed here In order to evaluate the proposed system more precisely improvements must be made in the area of output analysis For this project output analysis was done primarily by visual in
78. ent GPS position graphically on a local area map on the screen The program could also be used to review the collected data after it had been collected to disk The block diagram shown in Figure D 1 shows the basic flow of the software Program Start PEE CI LLLI D QU C J Interrupt Service Routines ii running asynchronous to main loop Clear Timer Flag Com Port ISR 100 Hz Timer ISR Clear GPS Flag Open Output Files Init VGA output Init Timer ISR i Init Com Port ISR Init Com Port Put new character into GPS message Set Timer Flag Is Timer Flag set Is GPS Message Complete Time to call BIOS Timer ISR Clear Timer Flag Sample DaqBook Save samples to disk Update screen Set GPS Flag Call BIOS Timer ISR Process GPS data Put new data on screen Save data to disk Remove Timer ISR Remove Com Port ISR Close Output Files Set Screen to Text Mode Is GPS Flag set Has a key been pressed Yes Was s key pressed Program Finish No Figure D 1 DOS data collection program software flow 89 Listed below is a fragment of the header maplog h This contains a number of define statements for I O port addresses screen size information etc This file also contains the type declarations for the se
79. ent them in a processor The discrete form of the above equations is x k Tv k cos 6 Tv TV k sin 8 ird 9 E T v E tan o t v k 1 la k o k 1 x METERS _ PER TICK T is the sample time 0 01 seconds in our case and is the discrete time step index 22 4 3 2 Configuration 2 GPS Receiver Gyroscope and Odometer GPS Receiver Sensor Fusion System Figure 4 7 Configuration 2 block diagram Our second configuration involved using a gyroscope for heading information instead of using a steering measurement This simplifies the model somewhat because we can essentially model the vehicle as a point This is because a gyroscope provides a direct measurement of angular velocity so we can ignore the front wheels and the length of the vehicle The dynamic equations for this model are X vcos 0 y vsin U v 0X METERS TICK where U is the gyroscope measurement in degrees second of angular change The discrete form of the dynamic equations is x k 1 x k Tv k cos 0 k y k 1 y k Tv k sin 0 k 0 k 1 20 k TU v k 1 a k o k 1 x METERS _ PER TICK Note that the value of at time 1 is dependent upon the current measurement and the previous value of This means that must be initialized at some point to a know value 23 4 3 3 Configuration 3 GPS Receiver Gyroscope and Accel
80. er BORDERCOL vscreen draw sensor vals SENSORCOL vscreen break toggle quickview mode if postview if quickview quickview 1 else quickview 0 break move cursor up calc_latitude 5000L break move cursor down calc latitude 5000L break move cursor left calc_longitude 5000L break gts move cursor right calc_longitude 5000L break Pats put time into key file if logging fwrite amp log_data ticks sizeof unsigned long 1 key_file break 99 default no key or invalid key break CleanUpTimer reset timer interrupt to old rate CleanUpComl reset com port interrupt ISR fclose key file close files fclose log file fclose gps file log file gps file key file NULl output status message if novga blit_string STATUSX STATUSY status_line BACKGNDCOL vscreen sprintf status_line Stopped s Q to quit log file name if novga blit string STATUSX STATUSY status line TEXTCOL vscreen else printf sWMn status line for user_key 0 user_key q amp amp user key Q get user key wait for if novga RestorePallette OldPallette restore old pallette set text reset to text mode goodbye 0 interrupt handler for timer ISR void interrupt far Handler void hsec new_sense 1 100th second ela
81. erest of keeping the software as simple and fast as possible while still providing some graphical feedback we used the video mode known as Mode 13 Many older DOS based video games use Mode 13 because it is very simple and easy to implement on any PC We implemented a very low level pixel plot routine and from it built routines to plot lines boxes and text This section will cover only the basics of Mode 13 operation Mode 13 is a 320x200 resolution graphics video mode which every graphics card supports Video memory for this mode is represented as a single contiguous segment of memory starting at location 40000 hex Each pixel is a single byte in the array so the entire screen is 64000 320x200 bytes in size Colors are produced by maintaining a palette of 256 color values The palette is a series of 256 three byte triplets defining the 58 red green and blue components for each entry Thus the mode is capable of representing 16 8 million 256 different colors but only 256 at a time Mazidi and Mazidi 1995 Initializing the video mode to Mode 13 is very simple We need only to put the value 13 hex into the ax CPU register and generate the BIOS interrupt 10 hex This is shown in the function set_vga void void asm pusha store the A register so we don t mess it up mov ax 0x0013 load 0x13 into the A register VGA mode int 0x10 call the BIOS interrupt to set mode 0x13 popa restore the A registe
82. erometer Sensor Fusion System Figure 4 8 Configuration 3 block diagram The third configuration considered involves the use of an accelerometer for acceleration measurement which can be used to determine relative speed and distance and a gyroscope again for heading information The dynamic equations for this configuration are shown here X y vsin 6 U v U where U is again the gyroscope measurement in degrees second and U is the 2 accelerometer reading in meters second The discrete form of these equations is shown here x k 1 6 k y k 1 y k Tv k sin 0 k 1 v k 1 v k TU k 24 As in configuration 3 this configuration has dependent the measurement and the previous value of 0 The value of v is calculated in a similar manner in this case Thus both 0 and v must be initialized to known values at some point 4 3 4 Configuration 4 GPS Receiver Gyroscope and Two Orthogonal Accelerometers GPS Receiver 2 Accelerometers Sensor Fusion System Figure 4 9 Configuration 4 block diagram This configuration is the only one in which redundant sensor data is present The gyroscope and one of the accelerometers both provide heading information in the model The dynamic equations that relate the measurements to the system variables are shown here vcos 0 y vsin U 6 23 0
83. es and performs point positioning with the corrected pseudoranges The use of the corrected pseudoranges improves positional accuracy Kaplan ed 1996 80 Appendix Geodetic to Ground Conversion Circumscribing circle quet Observer Equatorial plane Elliptical cross section Figure B 1 Detailed cross section view of earth for geodetic to ground conversion The GPS receiver returns data in the form of geodetic latitude longitude coordinates while the dead reckoning system returns relative ground coordinates with units of meters Therefore it is desirable to calculate values and such that meter C 1 degree latitude meter 2 C 1 degree longitude This allows us to convert the relative meter measurements from the dead reckoning system into degrees minutes seconds of latitude and longitude so that the data can be fused with the GPS data Note that assuming the Earth to be a perfect ellipsoid with a fixed equatorial radius a and eccentricity e these conversion rates are dependent only upon the current latitude measurement The first part of the derivation consists of calculating the coordinates 2 of a point on the earth s surface as indicated on Figure 81 B 1 and is taken from pages 26 29 from Methods of Orbit Determination by P R Escobal 1965 It is included here as a reference for the reader By inspection of Figure B 1 we have the follo
84. es between data sets such as close and far Instead the use of fuzzy logic specifically allows for the ambiguity of these data set boundaries in its operation Fuzzy logic allows a system to respond with a mixture of behaviors depending on the membership of the input data in various fuzzy sets Also rather than using a very complex set of nested if then else statements we can perform sensor fusion in a more structured and easily understood manner Consider the case involving the distance between the current position estimate and the GPS position as described in the rules listed in section 5 3 Using rule based fusion when the distance is small we assume the GPS position is accurate and we fuse the data appropriately However when the distance is large we assume the GPS receiver is suffering from some significant amount of error and we ignore it completely In general this works but there may be cases when valid GPS data is considered to be far from the current estimate position and is ignored GPS and inertial data continue to Distance cause the system to diverge from the fail to fuse GPS and point of error Threshold INS data Sensor data errors Time If we use fuzzy logic however this will not happen Suppose we use a simple membership function as shown here 1 m penne 0 9 9 0 09 9 9 0 9 9 0 0 9 0 00 00009 Distance Figure 5 6 Fuzzy membership functions for distance 37
85. ese distances A Navstar receiver anywhere on or near the surface of the Earth picks up the signals from four or more Navstar satellites A string of precisely timed binary pulses travels from each satellite to the receiver taking about one eleventh of a second The receiver estimates the signal travel time by subtracting the time from its internal clock from the time indicated by the 78 satellite when it transmitted the pulse This signal is then multiplied by the speed of light to obtain the estimated range to the first satellite If the clock in the receiver were perfectly synchronized with the clocks onboard the satellites three ranging measurements of this type would be required to obtain an accurate three dimensional position estimate However most Navstar receivers use inexpensive quartz crystal oscillators to measure the current time Consequently the receiver actually estimates the pseudo range false range to each Navstar satellite Since each range measurement is corrupted by the same timing error in the receiver s clock this error can be removed mathematically with the measurement of a fourth satellite pseudo range In addition to a three dimensional position estimate a Navstar receiver can calculate its velocity and heading along with the time of day and the date Logsdon 1992 A 3 System Segments The Navstar Global Positioning System is typically divided logically into three main pieces or segments These segments ar
86. essage message posnt t pos chan minutes Status Message message tempchar Status Message message integer seconds PACK8 Status Message message posn Status Message message posn l Status Message message posn 2 Status Message message posn 3 tempu4byte message posn 4 pos_chan gt seconds double tempchar double tempu4byte 1 0E 9 if scan_mode 0 return do not include position data unless asked PACK8 Status Message message posn Status Message message posn l Status Message message posn 2 Status Message message posn 3 temps4byte message_posn 4 gps_latitude temps4byte degrees double temps4byte MSECS_TO_DEGREES pos_chan gt latitude degrees TWOBYTE degrees if degrees lt 0 degrees fabs degrees minutes degrees TWOBYTE degrees 60 0 pos chan latitude minutes TWOBYTE minutes pos chan latitude seconds minutes TWOBYTE minutes 60 0 PACK8 Status Message message posn Status Message message posn l Status Message message posn 2 Status Message message posn 3 temps4byte message posn 4 gps_longitude temps4byte degrees double temps4byte MSECS_TO_DEGREES pos_chan gt longitude degrees TWOBYTE degrees if degrees lt 0 degrees fabs degrees minutes degrees TWOBYTE degrees 60 0 pos_chan gt longitude minutes TWOBYTE m
87. estimate position to the GPS position In this case we assume that some serious but temporary failure has occurred in the GPS receiver or the sensors such to cause our estimate to diverge so far from the true position that we can not fuse data Thus we decide to simply reinitialize our estimate with the GPS data and start the fusion process over again After adding the previous rules to our algorithm this rule never was executed in our sample data It is possible however that this event would occur so this last resort rule should be in place Note that all of the rules use terms such as very long time close and too far These values must be determined by experimentation and tweaking of parameters until the system obtains satisfactory results In a rule based system such as the one described above problems arise relating to the use of hard limits in place of terms such as close and too far If the data just barely crosses from one set such as close into another set such as the behavior of the fusion process can suddenly jump Another alternative which replaces hard thresholds with fuzzy thresholds is the use of fuzzy logic for the fusion process We describe a fuzzy logic system for sensor fusion in the following section 36 5 5 Fuzzy fusion of INS and GPS As mentioned in the previous section problems can arise from using hard or crisp thresholds to define boundari
88. esults may be obtained by integrating inertial sensor data and GPS sensor data using relatively simple sensor fusion techniques The techniques presented here focus around a loosely coupled configuration of GPS and inertial navigation sensors where the ultimate goal was to find the optimal weighting of the input from each sensor An eight state two input Kalman filter running at 100 Hertz was used to estimate the relative position of the vehicle between consecutive GPS samples As GPS samples arrived at approximately 1 Hz one of several sensor fusion techniques was used to determine the relative weightings for the GPS and inertial position estimates This project has demonstrated that good results are obtainable by using only a few relatively inexpensive inertial sensors and an inexpensive GPS receiver 74 References 1 1 g to 5 g Single Chip Accelerometer with Signal Conditioning Technical Paper Analog Devices 1996 2 Abrash M Michael Abrash s Graphics Programming Black Book Coriolis Group Books Boston 1997 3 Barshan B and Durrant Whyte H Inertial Navigation Systems for Mobile Robots IEEE Transactions on Mobile Robotics and Automation Vol 11 No 3 June 1995 4 Bennett David et al Visual C 5 Developer s Guide Sams Publishing Indianapolis 1997 5 Brey B 8086 8088 Microprocessor Architecture Programming and Interfacing Merrill Publishing Company Columbus 1987 6 Cham
89. ezoelectric rate sensor which refers to the main internal component of the gyroscope that allows it to determine angular velocity It operates on the Coriolis principle that means that a linear motion within a rotational framework will have some force that is perpendicular to that linear motion Miyazaki 1994 This simply means that in the case of automobile navigation the gyroscope is designed to measure the force perpendicular to the vehicle s forward motion which is proportional to angular velocity The Gyrostar is capable of a measurement range of roughly 80 deg sec and has a linearity 0 5 full scale which is sufficient for automobile applications Gyrostar 1994 The accelerometer that we used in the project is the Single Chip Accelerometer with Signal Conditioning The ADXLOS5 has an adjustable measurement range from 10 to 5g and an adjustable output scale from 200 mV g to 1 V g The entire sensor is encased in a single 10 pin TO 100 case and requires only a small circuit to set the adjustable range and scale and to filter the output as desired 10 to 5g Single Chip 1996 For our purposes we wish to reduce high frequency output such as that due to vibration and use only the lower frequency output such as that due to inertial effects of turning and acceleration This is accomplished by using the manufacturer recommended DC coupled connection which has a frequency response from dc 0 Hz to 1000Hz and measure
90. f a zero frequency DC component a low frequency component and a high frequency component The zero frequency component of the output signal is due to any offset in the signal which is intended in the design of the accelerometer signal plus an offset due to any slight misalignment in mounting or tilt of the vehicle The high frequency component is due to vibrations in the vehicle which is undesired in the signal Fortunately passive analog filtering can usually take out the high frequency component once a cut off frequency has been determined The zero frequency component must be taken care of in the navigation algorithm as a center point of the accelerometer In the ideal case we want only the component of the signal that is due to the movement of the vehicle on the road Another consideration in using accelerometers is that for determining heading one must first know angular velocity Acceleration normal perpendicular to the direction of motion relates speed and angular velocity Thus speed must be known to determine angular velocity and heading There are a couple substantial benefits to using accelerometers however First accelerometers can be purchased in a single chip package making them very small and cheap Second accelerometers are capable of measuring distance without any attachments to the vehicle itself In the design of a self contained INS package with no external encoder or tachometer attachment accelerometers are the only
91. f which is shown in Figure 5 1 With a loosely coupled integration the GPS pseudorange measurements are preprocessed by a Kalman filter internal to the GPS receiver which produces GPS derived geographic position and velocity as the receiver outputs Weiss 1996 The block diagram for a loosely coupled integration is shown in Figure 5 2 Because the Motorola Oncore GPS receiver has a built in Kalman filter and outputs the GPS derived position and velocity outputs we use the loosely coupled configuration in our experiments GPS Receiver Inertial Reference Data INS GPS System Nav Kalman Deltarange Filter Pseudorange and Figure 5 1 Tightly coupled GPS INS System GPS Receiver Front End Inertial Reference Data INS Processing filtering INS GPS Fusion Figure 5 2 Loosely coupled GPS INS System Pseudorange and INS GPS Kalman Filter System Nav Output 1 1 1 1 1 1 1 1 1 1 1 Deltarange 1 1 1 1 1 1 1 1 GPS Receiver 1 A simple block diagram of our implemented algorithm is shown in Figure 5 3 Inertial data Inertial sensor at input 100 Hz Sensor fusion at System output 1 Hz GPS data GPS collection at 1 input Hz Figure 5 3 Basic sensor fusion algorithm flow 28 We perform inertial navigation filtering at 100 Hz then fuse the inertial data with the GPS data at 1 H
92. gh great effort was spent in making both the DOS and Windows programs capable of all tasks each operating system is more suited for either real time data collection or post collection analysis DOS does not use the graphical user interface GUI that Windows uses and it does not directly support multi tasking so system resources are used one application exclusively addition DOS gives the programmer easier access to low level functionality of the system such as communications ports and direct VGA screen output This makes DOS much more suited to real time applications because we can use the system in a much leaner manner by telling the system exactly what to do and when to do it Conversely Windows provides high level abstractions of the system providing the programmer with many useful functions at the cost of a large a amount of overhead In addition Windows protects direct access to low level system devices such as communications ports making them more difficult and time consuming to work with For these reasons Windows is more suited for post collection analysis and visualization of the data Either system can do both collection and analysis but DOS is better at real time collection and Windows is better at graphical visualization Thus this is how we primarily used the software we collected data with the DOS software and did post collection analysis with the Windows software 6 2 DOS Software Development For
93. guage_border BORDERCOL vscreen break Mts decrease low range value for current sensor if curr_guage draw_guage_border BACKGNDCOL vscreen draw sensor vals BACKGNDCOL vscreen sen low curr guage 1 GUAGE DELTA if sen low curr guage 1 0 sen low curr guage 1 70 draw guage border BORDERCOL vscreen draw sensor vals SENSORCOL vscreen break YESS increase low range value for current sensor if curr_guage draw_guage_border BACKGNDCOL vscreen draw sensor vals BACKGNDCOL vscreen sen low curr guage 1 2GUAGE DELTA if sen_low curr_guage 1 gt sen_high curr_guage 1 sen_low curr_guage 1 sen_high curr_guage 1 10 draw_guage_border BORDERCOL vscreen draw sensor vals SENSORCOL vscreen break hats decrease high range value for current sensor if curr_guage draw_guage_border BACKGNDCOL vscreen draw_sensor_vals BACKGNDCOL vscreen sen_high curr_guage 1 GUAGE_DELTA if sen high curr guage 1 sen low curr guage 1 sen high curr guage 1 sen low curr guage 1 10 draw guage border BORDERCOL vscreen draw sensor vals SENSORCOL vscreen break ps increase high range value for current sensor if curr_guage draw_guage_border BACKGNDCOL vscreen draw sensor vals BACKGNDCOL vscreen sen high curr guage 1 GUAGE DELTA if sen_high curr_guage 1 gt 5000 sen high curr 11 5000 draw guage bord
94. hapter on software 11 3 5 GPS Receiver Perhaps the single most important piece of hardware in this system is the GPS receiver The output from the receiver is the only way we have any notion of our absolute position on the globe For information regarding some basic GPS fundamentals refer to Appendix C Figure 3 8 Motorola VP Oncore GPS Receiver from Motorola Oncore User s Guide The GPS receiver used in this project was a Motorola VP Oncore 6 channel receiver This version of the Oncore receiver communicates serially via a transistor transistor logic TTL interface with the host computer at 9600 baud The receiver has the capability to perform differential GPS DGPS given the appropriate input from a source such as a Coast Guard DGPS station We did not use this feature since it required additional hardware a Coast Guard beacon receiver and because differential correction signals are not available everywhere We used the GPS receiver in a very simple manner The receiver can be set such that it automatically sends its complete message once every second It is then the job of the host computer to detect the message and interpret it correctly From this data we know our current position time and status information for the receiver The receiver is capable of receiving and processing dozens of user commands but for our purposes the automatic once per second output is adequate For more information about the Motorola Binary Form
95. he given alternative in a system 4 1 System Variables Regardless of which sensor configuration we choose the desired output is the same At a minimum we want the best possible estimation of our current position at all times Additionally we may desire such information as speed heading and acceleration A simple dynamic model of the vehicle is shown in Figure 4 1 Figure 4 1 Simple 2D dynamic model of vehicle 4 2 Sensor Types At a bare minimum we desire lateral and longitudinal information about our vehicle Thus we need both lateral sensors and longitudinal sensors This section reviews 15 a variety of dead reckoning sensor types and the information that they can provide to a navigation system 4 2 1 Distance Encoders Distance encoders are very useful for automobile navigation because they are digital in nature A single rotation of the shaft on which an encoder is mounted produces a fixed number of pulses which can easily be counted By keeping a running count of all of the encoder pulses a navigation algorithm can know exactly what total distance has been traversed A first derivative results in roughly instantaneous speed and a second derivative results in roughly instantaneous acceleration However since we typically are interested in vehicle position which is a function of distance from the starting point the exact distance output from the encoder is very useful Figure 4 2 A typical small qua
96. his project was a Iotech DaqBook 100 It is a small box that sits outside the PC and attaches to the PC via a parallel cable All of our sensor inputs are connected into the three connectors on the back of the DaqBook one analog I O one digital I O and one pulse frequency high speed digital input Our two gyroscopes even though one was not operational two accelerometers and steering potentiometer all input to Status indicator lights the analog I O port The single distance Parallel PC interface encoder output we used either the odometer input or the ABS input at any one time fed into the pulse digital Figure 3 7 Close up picture of the DaqBook input The DaqBook analog inputs can 10 be set to operate in differential or single ended mode This means that analog signals be measured based on the potential between the single pin s input and the DaqBook s ground single ended or the signals can be measured based on the potential between adjacent pins differential In addition the board can be set to measure analog inputs as unipolar or bipolar In unipolar mode input voltages from 0 to 10 volts can be applied and in bipolar mode input voltages of up to 5 volts in magnitude in either polarity can be applied Each of the analog pins we set to operate in differential unipolar mode When the DaqBook is connected to a standard parallel port it supports up to 170 Kbytes sec of bi directional communication
97. his thesis Compare the fused output with the ideal output from the stored list of road segments Use this information to fine tune the sensor filtering and fusion parameters to produce a good output The iterative hill climbing or genetic algorithm methods could be implemented to automate the process of parameter optimization 5 Post process the GPS and inertial sensor data producing a map matched output from the system The output from the system can be evaluated by comparing the percentage of correctly traversed intersections on the map with the list of actual intersections traversed This system of evaluation can be used to fine tune the map matching algorithm to generate a reliable output based on the map matched position estimate If the output from the GPS inertial sensor fusion portion of the system has a large amount of error it may be required to manually reset the position estimate occasionally 6 After both the sensor fusion and the map matching parts of the system have been optimized the system could be set to evaluate its own performance during runtime Assuming that the map matched output is correct for some time interval while the output from the sensor fusion subsystem indicates that some sort of inertial drift is occurring this information could be used to dynamically adjust filtering and fusion parameters to account for this drift Such drift is known to occur as vibrations or temperature changes alter some sensor characte
98. ier is any unique identifier so that the push and pop statements can be matched This causes the compiler to pack only the myst ructure structure to the 1 byte boundary allowing us to read older files with no substantial code modification 6 3 5 Filter Implementation The actual implementation of the filter in the program was done using a set of matrix functions built on standard ANSI C routines which were taken primarily from Numerical Recipes in C Press et al 1988 We used ANSI C to aid in portability in case the need should ever arise to use the software on another platform For example one application may involve using the filter in a DSP or embedded microprocessor in a portable system which would most likely require ANSI C language compliance to compile 6 3 6 Data Visualization Data visualization refers to the ability to view the data streams going into and out of the program In this case we would like to be able to see the raw input data as it is processed which includes the sensor values and GPS data values Relevant GPS data includes the current unfiltered latitude and longitude heading velocity time and the 67 number of satellites locked simple graphical display for this data is shown in Figure 6 7 PostView Figure 6 7 GPS information view of post processing application The raw sensor values include the measurement values for the steering potentiometer both accelerometers and both gyroscopes
99. iew the data from the GPS and inertial sensors and displayed the map and processed output in a separate graphical display window The program follows the basic flow as shown in Figure 6 2 on page 43 with the inputs being from the hard drive pre collected data and the output to the screen The methods which operate at 1 Hz and 100 Hz are FilterlHz and Filter100Hz respectively OnIdle method is called by Windows and performs the necessary synchronization of FilterlHz and Filterl00Hz Listed below are several of these functions that actually did the filtering and fusion in the program The program contains a large amount of graphical interface code that is not relevant to the operation of the filter and has therefore been omitted from this listing OnIdle is called by Windows when it is not doing anything else BOOL CPostViewApp OnIdle LONG lCount if m pMainPage m RunCheckVar if running m RunCount tt read next sensor value from file time data is global time if m pLogFile Read amp m CurrSensorData sizeof T LOG DATA sizeof T LOG DATA m_pMainPage gt SetRunState FALSE return TRUE first time execution read GPS value no matter what if m_NextGPSTime set the current GPS data to the first GPS data in the file even if it isn t quite time yet if m pGPSFile Read amp m NextGPSTime sizeof unsigned int sizeof unsigned int m pMainPage SetRunState FALSE return TRUE
100. if strcmp var sensor fscanf cfg file d 684 1 34 gt 0 amp amp j 5 fscanf cfg file d d amp sen low jl amp sen high jl1 else fgets var 20 cfg_file eat remainder of unknown config line fclose cfg_file cfg_file NULL else goodbye 1 unknown command line parameter gt exit if postview amp amp logging goodbye 1 doing nothing gt exit bioscom 0 0xE3 0 set COMI to 9600 N 8 1 for GPS receiver if logging sprintf status_line Waiting s to go log file name ifndef NOBOOK init daqbook initialize DaqBook bioscom 0 0xE3 0 set COMI to 9600 N 8 1 for GPS receiver set to gps synchronize trigger times get odometer count initialize odometer counter daqCtrMultCtrl DmccLoad 1 1 0 0 0 dendif odometer total prev odometer total 0 else sprintf status_line Waiting s G to go log file name curr old data 0 if novga set_vga switch to vga 320x200 graphics mode GrabPallette OldPallette save the old palette Set pallette set new palette do initial screen setup here draw map border BORDERCOL vscreen draw sensor border BORDERCOL vscreen draw gps border BORDERCOL vscreen draw guage border BORDERCOL vscreen draw strength border BORDERCOL vscreen blit string STATUSX STATUSY status line TEXTCOL vscreen if mapping draw map BLUE
101. if p x mapullong amp amp p x lt maplrlong amp amp p y mapullat amp amp p y maplrlat return 1 return 0 close all files free all allocated data and exit the program void goodbye int usage if log_file NULL fclose log file gps file NULL fclose gps file map_file NULL 1 _ 11 if Ist if cfg_file NULL fclose cfg file if key_file NULL fclose key file if sia LET if debug NULL fclose map file old data NULL farfree old data map roads NULL farfree map roads map segments NULL farfree map segments print a little message if usage flag is set if usage printf Usage n printf maplog p 1 filename m mapfile novga no extensions pr ingpt Exiting yn exit 0 draw all of the map lines the map window void draw_map unsigned int color unsigned char far vscreen int i for i 0 i lt num_map_segments itt if in view map segments i pl in view map segments i p2 map line ll to screen map segments i pl1 11l to screen map segments il p2 color vscreen 109 D 2 Windows Program Listings The Windows 95 98 NT program that was written for this thesis was used primarily to process the previously collected data using the implemented filter and to generate and display the filtered output This program used a simple dialog based application structure to v
102. inutes pos chan longitude seconds minutes TWOBYTE minutes 60 0 templong pos chan longitude seconds pos chan longitude minutes 60 0 abs pos_chan gt longitude degrees 3600 0 gps longitude long templong 1000 0 if pos chan longitude degrees 0 0 gps longitude 0 gps longitude PACK8 Status Message message posn Status Message message posn l Status Message message posn 2 Status Message message 3 temps4byte message_posn 4 pos_chan gt datum_height double temps4byte 100 0 PACK8 Status Message message posn Status Message message posn l Status Message message posn 2 Status Message message 3 temps4byte message posn 4 pos_chan gt msl_height double temps4byte 100 0 tempchar Status_Message message_posnt t pos chan velocity double tempchar lt lt 8 Status_Message message_posnt 100 0 tempchar Status_Message message_posntt pos_chan gt heading double tempchar lt lt 8 Status_Message message_posnt 10 0 tempchar Status_Message message_posnt t pos_chan gt current_dop double tempchar 8 Status Message message posn 4 10 0 pos chan dop type Status Message message _ pos chan visible sats Status Message message posn tt pos chan sats tracked Status Message message posn tt for i 0 lt NUM CHANNELS i pos chan channel i svid Status Message messag
103. ion Chambers 1995 In the case of our system the fitness function is a measurement of how closely the fused system output matches the true vehicle position However since it is nearly impossible to precisely measure the true vehicle position 41 during data collection the fitness function is actually a measurement of how closely the fused system output is to the road segments on which the vehicle traveled There are several choices for possible fitness functions measuring position estimate deviation from the actual roadway positions Some of these are e Largest perpendicular deviation from road segment throughout entire run e Average perpendicular deviation from road segments for entire run e Mean Square perpendicular deviation from road segments for entire run We chose the last method the mean square deviation from road segments for the entire run This fitness function takes into account all of the data points for the entire run and places more weight on larger error values That is one set of parameters may produce a larger average position error but it may still be consider more fit than another set of parameters that has more large singular position errors Here we examine a few methods by which these parameters may be updated to produce a more optimal system behavior These methods include manual optimization gradient descent methods and genetic algorithms 5 6 1 Manual Optimization Manual parameter optimization refe
104. itialized to be the same as the state noise matrix Q The initial state vector must be initialized with some data values that represent absolute measurements such as position and heading These data could be known and placed into the state vector in advance if the vehicle s starting point is know for example or the data could be obtained during run time and placed in the matrix before the filter begins operation In our case we chose to obtain the data during run time because we did not know our exact starting point This initial information initial latitude longitude and heading was obtained from the GPS received once it had gotten an initial position fix 5 4 Rule based fusion of INS and GPS Rule based sensor fusion refers to the integration of Kalman filtered INS data and the raw GPS position and heading information based on a set of rules thresholds and weights These rules are designed to compensate for errors in the GPS data such as drift and multi path while still using the GPS data in an effective manner to update our position estimate For example we found that when the vehicle is stopped the GPS receiver tends to drift around unpredictably and widely vary the heading value while remaining close to the current position If we were doing simple weighting of GPS and INS data our 34 position estimate would follow the GPS output around and end up with completely wrong heading value Thus we added a rule If we are n
105. ition into rom offset yc lt lt 8 lt lt 6 get offset position on the screen for y 0 y lt CHAR_HEIGHT y bit_mask 0x80 for x 0 x lt CHAR_WIDTH put the character on the screen bit by bit if work char amp bit mask vscreen offset x unsigned char color bit mask bit mask 1 offset SCREEN_WIDTH work_char initialize the DaqBook to the desired mode void init_daqbook void daqSetErrHandler daq error establish error handler daqInit LPT1 7 connect to daqboook dagAdcSetTag 0 disable tagged ADC data daqCtrSetMasterMode 1 DcsF5 0 0 0 set counter master mode string counter 1 and 2 for 32 bit event counting daqCtrSetCtrMode 1 DgcNoGating 1 DcsSrcl1 0 0 1 0 1 DocInactiveLow daqCtrSetCtrMode 2 DgcNoGating 1 0 0 0 1 0 1 DocInactiveLow daqCtrSetLoad 1 0 daqCtrSetLoad 2 0 daqCtrMultCtrl DmccLoad 1 1 0 0 0 daqCtrMultCtrl DmccArm 1 1 0 0 0 error callback function for the DaqBook functions void far pascal daq error int error elrseri printf nError Program aborted nDaqBook 100 Error Ox xMn error code farfree old data exit 1 detect GPS receiver and synchronize time to it void pc_set_to_gps void int i 0 gps_found 1 for 1 0 1 lt 5000 amp amp get_gps_data it delay 1 if i lt 5000 settime amp gps time else no gps receiver
106. iveness of their system based on the system output s deviation from the true path which was obtained by digitizing the path from an overhead map Ren Da and Ching Fang Lin 1995 use the State Chi Square Test and the ARTMAP Neural Network to perform failure diagnosis in a GPS INS integrated navigation system They tested their system by means of computer simulation and demonstrated the detection of soft failures by the tests 2 3 Contribution of This Work Several different types of systems have been used to generate an enhanced position estimate based on data from multiple sensors As is the case with all systems that use inertial sensors only those systems that were mentioned in section 2 1 are limited to provide only relative position and heading information from some arbitrary starting point The work of Ramjattan and Cross 1995 is actually very similar to the work that has been done for this thesis However this thesis goes further than to provide information only about how to fuse inertial and GPS data to produce an enhanced output This thesis examines several options for methods of fusing the inertial and GPS data such that other researchers can use this work as a starting point in their research In addition a unique map matching algorithm is presented that can be used to further enhance system output in areas where the roadways are known Map matching is a technique that has been around for a long time but has not been exploite
107. lated initialization allocate vectors and matrices xk vector 1 8 xkml1 vector 1 8 yk vector 1 2 Ak matrix 1 8 1 8 Kk matrix 1 8 1 2 C matrix 1 2 1 8 Pk matrix 1 8 1 8 1 matrix 1 8 1 8 Pkml matrix 1 8 1 8 R matrix 1 2 1 2 Q matrix 1 8 1 8 mtempl matrix mtemp2 mat rix mtemp3 mat mtemp4 mat rix mtemp5 matrix 1 NNN CO oS 404 oS mtemp6 matrix mtemp7 matrix mtemp8 matrix mtemp9 vector 1 2 mtemp10 vector 1 2 mtempll vector 1 8 mtemp12 vector 1 8 mtemp13 matrix 1 8 mtemp14 matrix 1 8 fill in intial values for Ak C Pkml1 Q R leave xkml all O s until good GPS initialize state transition matrix for 1 1 1 lt 8 1 1 3 lt 8 Ak i j Pkm1 i 3 Q i j 0 for i 1 i lt 2 i for j 1 j lt 2 j Ri j 0 for j 1 j lt 8 j Cli j 0 for 1 1 lt 8 1 Ak i i 1 3 4 DELTA TIME Ak 3 5 SQR DELTA TIME 2 4 5 DELTA TIME 6 7 DELTA TIME Ak 6 8 SQR DELTA_TIME 2 Ak 7 8 DELTA_TIME initialize observation matrix 1 4 1 C 2 6 1 initialize variance of initial errors in Pkml for i 1 i 8 i Pkml i i 0 02 initialize measurement noise matrix R R 1 1 2 61
108. le counting the length of time that has been small is needed to determine if our estimate is in error Once all of the variables have been fuzzified they must then be processed and defuzzified A common method of processing fuzzy data is simply to use the MIN function That is the combination fuzzy AND of two or more membership values is the 38 minimum of the values Consider the above example where 4 0 65 and Loose d 0 35 In addition suppose that the fuzzification of the current vehicle speed s resulted in Hp 5 0 55 and s 0 50 Consider the previously mentioned rule If Distance is Close and Speed is Fast then po is Large When processing this rule we consider the membership values of each of the conditionals to determine the output In this case where U d 0 35 S 0 55 the output for the rule is the minimum of the two membership functions resulting in a value of 0 35 The value 0 35 represents this rule s effect on making V to be Large Similarly consider the rule If Distance is Far and Speed is Slow then Po is Small Because 4 0 65 and p 5 0 50 the output for this rule is 0 50 In this case the value of 0 50 represents this rule s effect on making to be Small One can easily see that given these two rules and these membership values the fuzzy output is going to be more Small than it is Large Once the outputs from all such ru
109. les have been computed they must be combined and then defuzzified One method that is commonly used for defuzzification is the centroid method Consider the simple membership function for Yos shown below Rule2 0 50 Rulel 0 35 0 1 Figure 5 8 Output membership function for position weighting value If there were multiple rules that produced a value for these would need to be combined at this point using a fuzzy OR operation In this method of fuzzy processing and defuzzification the fuzzy OR operation is implemented simply by finding the maximum of all membership values being combined In this example however there is only one rule contributing a and only one rule contributing a Miroz 39 Combining the value for and the value for to produce an output membership function for Y os is done by truncating each of the membership sets at the corresponding membership value and merging the sets into a single fuzzy output set This set is the fuzzy output for the variable y based on all of the fuzzy rules in the system For example the above membership function for V ps based the given values for and would become 0 V pos 1 Figure 5 9 Fuzzy output of the system After the fuzzy output of the system has been determined the only remaining task is to defuzzify the output to produce a crisp number that can be used as a weight to fuse the
110. little or no modification 6 2 1 Programmable Interval Timer This section covers details regarding the implementation of a very precise method of timing the main loop of our DOS based program We explain some nitty gritty details here not to be read by the faint of heart Note the following discussion is based on information that is several years old The actual chip level implementation may be different in modern PCs but they still function in accordance with this discussion Every PC has on its motherboard an Intel 8253 Programmable Interval Timer PIT The PIT chip has three channels each of which is responsible for a different task Channel 0 is responsible for updating the system clock It is usually programmed to generate about 18 2 ticks a second Interrupt 8 which is serviced by the PC is generated for every tick Typically the operating system services the interrupt in order to maintain the current time of day estimate Channel 1 controls DRAM memory refreshing DRAM memory requires periodic refreshing to prevent information loss for which this channel is responsible Channel 2 is connected to the PC speaker and is 53 typically used to generate a square wave so a continuous tone is heard Brey 1987 Roden 1992 Mazidi and Mazidi 1995 The channel of interest for us is channel 0 the channel used to update the system timer As mentioned above channel 0 is typically set to tick at roughly 18 2 Hz much slower than
111. ltage divider with 5 volts on the input and the output signal going into the data acquisition board We mounted the sensor such that when the steering wheel was turned all the way to the right it output 1 2 volts and when the wheel was turned all the way to the left it output 4 5 volts The full scale of 0 to 5 volts was not used because this would require the potentiometer to be mounted such that it was pulled to each extent of operation during use This could potentially wear down the sensor and even break it if it were over extended by a small amount repeatedly The mounted potentiometer is shown in Figure 3 4 Cable to data acquisition hardware Potentiometer Pull string String around steering column Steering column Figure 3 4 Mounted steering potentiometer 3 3 Inertial Sensors We used two types of inertial sensors on this project gyroscopes and accelerometers Both types have been in use for navigation applications for several years Recent advances in gyroscope technology in particular have allowed smaller cheaper and more accurate gyroscopes to be offered making INS solutions more practical Two different gyroscopes were mounted on the sensor board initially for purposes of evaluating the performance of each One of them broke shortly into the project thereby failing its test and we will not discuss it further The gyroscope that we relied on exclusively was the Murata Gyrostar The Gyrostar is a vibratory pi
112. ly if m GPSPosWeight 0 025 m_NumGoodNoWeight else m NumGoodNoWeight 0 perform median filtering on the GPS heading outputs to eliminate glitches m HeadingFilt m CurrHeading 2m CurrGPSData heading m CurrHeading m CurrHeading 1 HEADING FILT SIZE double SortHeading HEADING FILT SIZE for int i 0 i lt HEADING_FILT_SIZE i SortHeading i m_HeadingFilt il qsort SortHeading HEADING_FILT_SIZE sizeof double dcomp m GPSHeading SortHeading HEADING FILT SIZE 1 2 weighted sum of GPS position and current position using resultant GPSPosWeight xkm1 1 m_GPSPosWeight m_GPSLongitude 1 0 m_GPSPosWeight xkm1 1 xkm1 2 m_GPSPosWeight m_GPSLatitude 1 0 m_GPSPosWeight xkm1 2 assumed heading and GPS sensor heading are 90 degrees out of phase and inverted double gpshead 90 0 m GPSHeading get GPS heading and current heading within 180 degrees and both positive so the average does not suffer from wrap around errors while xkm1 3 gpshead 180 0 gpshead 360 0 while gpshead xkm1 3 gt 180 0 gpshead 360 0 117 weighted sum of GPS heading and current heading using resultant GPSHeadingWeight xkm1 3 m_GPSHeadingWeight gpshead 1 m_GPSHeadingWeight xkm1 3 else GPS data is invalid so ignore it altogether plot the filtered position in the output window if m_fInitGPS amp amp m_fxkValid m_pMapDlg gt PlotFiltPos CPoint int xk 1 int xk
113. mbers 5 GPSHeadWeight SetMemberFunc ZERO 5 0 0 0 03 0 05 ZERO 5 GPSHeadWeight SetMemberFunc SMALL 5 0 02 0 05 0 25 0 3 SMALL 5 GPSHeadWeight SetMemberFunc MED 5 0 25 0 3 0 4 0 5 MED 5 GPSHeadWeight SetMemberFunc LARGE 5 0 4 0 i 0 8 0 95 LARGE_5 GPSHeadWeight SetMemberFunc ONE 5 0 9 0 95 1 ONE 5 GPSPosWeight SetNumMembers 5 GPSPosWeight SetMemberFunc ZERO 5 0 0 0 001 0 001 ZERO 5 GPSPosWeight SetMemberFunc SMALL 5 0 0 002 0 015 0 02 SMALL 5 GPSPosWeight SetMemberFunc MED 5 0 015 0 02 0 02 0 03 MED 5 GPSPosWeight SetMemberFunc LARGE 5 0 02 0 03 0 05 0 10 LARGE 5 GPSPosWeight SetMemberFunc ONE 5 0 999 0 999 1 1 ONE 5 NumNoGood SetNumMembers 3 NumNoGood SetMemberFunc SMALL 3 0 0 25 35 SMALL 3 NumNoGood SetMemberFunc MED 3 25 35 2500 3500 MED 3 NumNoGood SetMemberFunc LARGE 3 2500 3500 1000000 1000000 LARGE 3 DistRatio SetNumMembers 2 DistRatio SetMemberFunc SMALL 2 0 0 1 0 1 5 SMALL 2 DistRatio SetMemberFunc LARGE 2 1 0 1 5 1000000 1000000 LARGE 2 m FilterInitialized TRUE 113 void CPostViewApp StopFilter void free all of the allocated matrices free vector xk 1 8 free vector xkm1 1 8 free vector yk 1 2 free matrix Ak 1 8 1 free matrix Kk 8 1 free matrix free matrix free matrix x y A 8 K 2 Gdy 2 51 58 Pk 1 8 1 8 Pk1 1 8 1 8 P 18 R Q x
114. n estimation capabilities of a system by incorporating other sensor and data technologies including Kalman filtered inertial navigation systems rule based and fuzzy based sensor fusion techniques and a unique map matching algorithm ii Contents Tal MOTIVATION eene EC ERE e d 1 1 2 SCOPE AND STRUCTURE THESIS ccccesssecccvsnscscceveccenscsccensccccenscsccensnaccsnscessenaccssensesssenscsseenenacs 1 2 L INERTIAL NAVIGATION ca 3 MX GST BUSION EET 4 2 3 CONTRIBUTION OF THIS 4 SM PEST VEHICDE eee terae E RS DH EE RE 5 3 2 DEAD RECKONING 5 5 8 6 3 3 INERTIAL 5 5 8 0200 1 ene 8 3 4 PAMA A COUS ON B OARD gait esses 10 GBS RECEIVER tA ea et Le 12 ERD S ONNE EOIN LOHN EI 14 MODELING AND SENSOR FUSION ALGORITHMS cccccscsssssssssssssssssssssssscssssssssssssssssessssses 15 4 I SYSTEM VARIABEBBS
115. ned char far vscreen unsigned int line_offset int index line_offset y1 lt lt 8 y1 lt lt 6 x pixel location in memory for index 0 index lt y2 yl indext for each row in the line vscreen line_offset color set pixel value in memory line_offset 320 increment to next line draw a colored line from point pl to p2 into video memory location vscreen void line int xl int yl int x2 int y2 unsigned int color unsigned char far vscreen int i d1lx dly d2x d2y u s v m n u x2 x1 overall run of the line 2 1 overall rise of the line dlx sgn u line top to bottom or bottom to top dly sgn v line left to right or right to left d2x sgn u line top to bottom or bottom to top d2y 0 m abs n abs v if m lt n af slope lt 1 7 d2x 0 d2y sgn v m abs v n abs u 5 2 for 1 0 1 lt 1 plot single pixel the screen only if within screen bounds if yl gt 0 amp amp 1 lt 199 amp amp gt 0 amp amp 1 lt 319 vscreen yl 8 yl 6 x1 color S increment to next pixel location if s gt m S m xl 1 yl dly 1 xl d2x yl 2 draw a colored line from point pl to p2 into video memory location vscreen void map_line POINT pl POINT p2 unsigned int color unsigned char far vsc
116. ng shaft measurement despite it initially seeming to be a good alternative First like a distance encoder or tachometer a steering measurement device such as a potentiometer must be mounted to the shaft of the steering wheel This mounting process is generally not trivial and must be calibrated and adjusted for each particular vehicle Additionally in using a measurement of the front wheel angle to calculate angular velocity we must know the length of the vehicle Using a steering shaft measurement requires substantial calibration for the vehicle on which it is used However decent results can be obtained from a steering measurement when it is properly used 20 4 3 Sensor Configurations For this project several sensor configurations were considered when designing the filtering algorithm Four different configurations are presented here chosen mainly because they each include a subset of the sensors with which we began the project Any of the configurations may be modified easily to include additional sensors Note that every configuration has a GPS receiver because it is our only means of absolute positioning The only difference then is amongst the variety of dead reckoning sensors that were chosen for the particular configuration In this section we are focusing on the configurations of sensors rather than the sensors themselves Thus for modeling purposes we will assume that every sensor has accurate noiseless signal outputs
117. nsor data that is collected and stored to disk Maplog h ifndef _MAPLOG_H define _MAPLOG_H define define define define define define define define define define define define PALETTE_MASK PALETTE_REGISTER_RD PALETTE_REGISTER_WR PALETTE_DATA VGA256 0x13 TEXT MODE 0x03 CHAR WIDTH 8 CHAR HEIGHT 8 ROM CHAR SET SEG 0 000 ROM CHAR SET OFF OxFA6E SCREEN_WIDTH 320 SCREEN_HEIGHT 200 0x3C6 0 3 7 0x3C8 0x3C9 5 ti typedef struct RGB_color_typ unsigned char red unsigned char green unsigned char blue RGB color RGB color ptr typedef struct log data st unsigned long ticks unsigned long odometer total unsigned int gyrol unsigned int gyro2 unsigned int accelf unsigned int accelr unsigned int steer T LOG DATA typedef struct point 554 long x y POINT typedef struct road segment st POINT pl p2 long road ROAD SEG define define define define Ox3F8 Ox3FD Ox3F9 Ox3FC GPS PORT DATA GPS PORT STATUS GPS PORT INTR GPS PORT CONT define define define COMIINTR PIT FREQ 0x08 0x0C 0x1234DDL dendif addr addr addr addr mode value width pixels height pixels Character segment in ROM Character offset in segment Default screen width pixels Default screen height pixels
118. nversely we could perform sensor fusion of GPS and inertial data at 1 Hz whenever GPS data is received and rely on inertial data alone to carry us between GPS samples It is the second method we chose to implement primarily because we felt the simpler approach was better and fusion at 1 Hz would be adequate for our purposes This chapter will cover the general approach of our algorithm as well as a description of our Kalman filter for inertial reckoning Then we describe two methods of determining the sensor fusion weights a simple rule based method and a more robust fuzzy logic based method Then a simple method for map matching is described which may be implemented if accurate latitude longitude road data is available for a region 5 1 General Approach There are two basic types of methods for integration of GPS and INS data in a system These are tightly coupled and loosely coupled Internal to the GPS receiver a set of pseudorange measurements is obtained for position estimation A pseudorange is a distance estimate from the GPS receiver to one satellite before being error corrected The straightforward and most optimal approach for integrating GPS with an INS is to directly utilize GPS pseudorange measurements from the GPS receiver to correct INS error growth with a specially designed Kalman filter that models the INS errors and the 27 GPS measurement geometry This is a tightly coupled integration technique a block diagram o
119. o creating fuzzy rules we chose to simply make fuzzy versions of the rules that we had determined using rule based fusion of the rules used in our application are shown here if Distance is SMALL and Velocity is FAST then GPSWeight is LARGE if Distance is MED and Velocity is FAST then GPSWeight is MED if Distance is SMALL and Velocity is MED then GPSWeight is MED if Distance is MED and Velocity is MED then GPSWeight is SMALL if Distance is SMALL and Velocity is SLOW then GPSWeight is SMALL if Distance is MED and Velocity is SLOW then GPSWeight is VERY SMALL if Distance is LARGE and NumNoGood is SMALL then GPSWeight is VERY SMALL if Distance is LARGE and NumNoGood is MED then GPSWeight is VERY SMALL if Distance is LARGE and NumNoGood is LARGE then GPSWeight is VERY LARGE 5 6 Fusion Parameter Optimization For an effective and reliable filter implementation the filter parameters are equally important as the governing system of equations Without good filter and fusion parameters the system will never be able to operate as expected Fusion parameters include values such as those that define the threshold values for the rule based fusion implementation and those that define the fuzzy boundaries in the fuzzy fusion implementation Appropriate or inappropriate choice of decision parameters will cause the system to perform better or worse as measured by some relevant objective or fitness funct
120. o print blit_string PCTSTRPOSX PCTSTRPOSY pct_str BACKGNDCOL vscreen pct str 0 char pct_val 3600000L 0 pct str 1 char pct_val 3600000L 360000L 0 pct str 3 char pct_val 360000L 60000L 0 pct str 4 char pct val 600001 6000 0 pct str 6 char pct val 6000 1000 4 0 pct str 7 char pct val 1000 100 0 0 97 blit_string PCTSTRPOSX PCTSTRPOSY pct_str TEXTCOL vscreen blit string TSGPSSTRPOSX TSGPSSTRPOSY tsgps str BACKGNDCOL vscreen tsec since gps int log data ticks last gps hsec 10 tsgps_str 0 char tsec since 9 1000 100 0 tsgps str 1 char tsec since 9 100 10 0 tsgps_str 3 char tsec since 9 10 0 blit string TSGPSSTRPOSX TSGPSSTRPOSY tsgps str TEXTCOL vscreen break case 6 time xxxxx 6 seconds update speed string if old_speed curr_speed blit_string SPDSTRPOSX SPDSTRPOSY speed_str BACKGNDCOL vscreen old_speed curr_speed speed_str 0 old_speed 1000 100 0 speed_str 1 old_speed 100 10 0 speed_str 2 old_speed 10 0 blit_string SPDSTRPOSX SPDSTRPOSY speed_str TEXTCOL vscreen break case 8 time xxxxx 8 seconds gt update lat long strings if old_calc_long calc_longitude old_calc_lat calc_latitude blit_string LATSTRPOSX LATSTRPOSY lat_str BACKGNDCOL vscreen blit_string LONGSTRPOSX LONGSTRPOSY long_str BACKGNDCOL
121. om port ISR instead of simply setting a flag each time a character is received and having the main program handle it This is because our main program is running at 100 Hz due to the timer interrupt while we receive data bytes at nearly 10 times that rate However the com port input is in bursts of 68 characters at 1 Hz This means that our program has time 57 to run at least 100 Hz loop and interpret the received message before the next message is begun Despite this long discussion of the ISR the actual implementation is quite simple The com port ISR code is shown here void interrupt far ComlHandler void gps char inportb GPS PORT DATA retrieve data from com port if new_gps switch gps_report_idx depending on what state we re in case 0 case 1 waiting for message beginning if gps_char gps_report gps_report_idx gps_report_idx break case 2 waiting for character if gps_char B gps_report_idx 0 else gps_report gps_report_idx B gps report idx break default waiting for remainder of message gps_report gps_report_idx gps_char gps_report_idx break if gps_report_idx 68 if message complete gps_report_idx 0 reset message index for next message new gps 1 set flag to indicate complete message outp 0x20 0x20 clear interrupt 6 2 3 VGA Output In the int
122. onale for this is that two bit shifts and an addition are faster than a multiplication The code for blit bit is shown here draw a single colored pixel into the video memory at location vscreen void blit bit int x int y unsigned int color unsigned char far vscreen vscreen y lt lt 8 y lt lt 6 x color set pixel color value in memory Based on this simple function for setting a pixel value in video memory we can easily make functions that efficiently draw horizontal or vertical lines into video memory draw a colored horizontal line into the video memory location vscreen void h_line int xl int x2 int y unsigned int color unsigned char far vscreen _fmemset char y lt lt 8 y 6 x1l color x2 x1 41 draw a colored vertical line into the video memory location vscreen void v_line int yl int y2 int x unsigned int color unsigned char far vscreen unsigned int line_offset int index 1 1 lt lt 8 1 lt lt 6 x pixel location memory for index 0 index lt y2 yl index for each row in the line vscreen line_offset color set pixel value in memory line_offset 320 increment to next line Drawing lines with arbitrary slopes in an efficient manner is a little more complicated As with the bit_blit function we would like our line drawing function to make no multiplies or divides f
123. or efficiency purposes To accomplish this we use a line drawing technique known as Bresenham s Algorithm Abrash 1997 draw a colored line from point pl to p2 into video memory location vscreen void line int xl int yl int x2 int y2 unsigned int color unsigned char far vscreen int i d1lx dly d2x d2y u s v m n u x2 x1 overall run of the line v y2 yl overall rise of the line dlx sgn u line top to bottom or bottom to top dly sgn v line left to right or right to left d2x sgn u line top to bottom or bottom to top d2y 0 m abs n abs if m lt n if slope lt 1 2 0 d2y sgn v m abs v 60 n abs s m 2 for 1 0 1 lt 1 plot single pixel on the screen only if within screen bounds if yl gt 0 amp amp 1 lt 199 amp amp gt 0 amp amp 1 lt 319 vscreen yl 8 yl 6 x1 color S n increment to next pixel location if s gt m S m xl 1 yl dly else xl d2x yl d2y 6 2 4 Data Visualization By building several functions based on the simple blit bit function we are able to produce a somewhat crude but useful graphical interface for our program We output some status information such as BIOS time running time and file name as well as the sensor outputs and much of the GPS message in graphical form If we properly define a
124. osoft Foundation Class MFC so any examples shown may be specific to this programming environment 6 3 1 Multi Threading An important difference between DOS based and Win95 based programming is the native support of multi threading under Win95 Multi threading is the ability of the operating system to run two pieces of code simultaneously In most PC systems the central processing unit CPU is not capable of running two pieces of code at exactly the same time Instead the operating system performs time slicing where CPU time is divided up and each section of code sometimes called a thread or process depending on the context is given a portion of the CPU s time Thus each thread thinks it has the whole machine to itself when it is really sharing the CPU time with several other threads or processes There are many details of multi threading that occur on the operating system level such as virtual address spaces and register swapping that are beyond the scope of this thesis and therefore will not be discussed Multi threading not only allows other applications to run on a system at the same time our application is running but it also allows us to use multiple threads within a single application In Win95 programming it is quite common to use a single user interface thread and a worker thread The main purpose of this is to allow the worker thread to function even while the user interface thread is busy Bennett et al 1997 For exampl
125. outp 0x40 0x00 outp 0x40 0x00 setvect TIMERINTR BIOSTimerHandler reset com port ISR function void CleanUpComl void outportb GPS_PORT_INTR gps_port_intr_set setvect COMLINTR BIOSComlHandler take the current palette and put it into memory void GrabPallette unsigned char Pall 256 3 int loopl for 100 1 0 100 1 lt 256 1 1 outp 0x03C7 100p1 Pall loop1 0 inp 0x03C9 Pall loop1 1 inp 0x03C9 Pall loop1 2 inp 0x03C9 I take the palette from memory and make it current void RestorePallette unsigned char Pall 256 31 int 1 1 for loopl 20 100 1 lt 255 1 1 outp 0x03C8 100p1 outp 0x03C9 Pall loop1 0 outp 0x03C9 Pall loop1 1 outp 0x03C9 Pall loop1 2 set a single palette member void Set Palette Register int index RGB color ptr color outp PALETTE MASK Oxff outp PALETTE REGISTER WR index outp PALETTE DATA color red outp PALETTE DATA color green outp PALETTE DATA color blue draw a colored horizontal line into the video memory location vscreen void h_line int xl int x2 int y unsigned int color unsigned char far vscreen int frequency _fmemset char y lt lt 8 y lt lt 6 x1 color x2 x1 1 101 draw colored vertical line into the video memory location vscreen void v_line int yl int y2 int x unsigned int color unsig
126. pTimer void CleanUpComl void __ interrupt Handler void __ interrupt ComlHandler void set pallette void GrabPallette unsigned char Pall 256 3 RestorePallette unsigned char Pall 256 3 Set Palette Register int RGB color ptr h line int int int unsigned int unsigned char far v line int int int unsigned int unsigned char far line int int int int unsigned int unsigned char far map line POINT POINT unsigned int unsigned char far fill box int int int int unsigned int unsigned char far set vga void set text void set palette void fill screen unsigned int unsigned char far blit char int int char int unsigned char far blit bit int int unsigned int unsigned char far init daqbook void _far pascal daq error int error code pc set to gps void get time void get odometer count void get user key void int gps serial wait for next void int get gps data void void void void void void void void void void void void void void void clear gps serial void Pos Status Data Decode unsigned char T POS CHAN STATUS char draw map border unsigned int unsigned char far draw sensor border unsigned int unsigned char far draw gps border unsigned int unsigned char far draw guage border unsigned int unsigned char far draw sensor vals unsigned int unsigned char far draw map unsigned int unsigned char far blit string int
127. psed set new data flag clock_ticks counter if clock_ticks gt 0x10000L clock ticks 0x10000L BIOSTimerHandler else outp 0x20 0x20 interrupt handler for com port ISR void __interrupt far ComlHandler void gps_char inporthb GPS_PORT_DATA if new_gps switch gps_report_idx case 0 case 1 if gps_char gps_report gps_report_idx gps_report_idx break case 2 if gps_char B gps_report_idx 0 else gps_report gps_report_idx B gps_report_idx break default gps_report gps_report_idx gps_char gps_report_idx break if gps_report_idx 68 gps_report_idx 0 new_gps 1 outp 0x20 0x20 set the timer rate and ISR function 100 void SetTimer void interrupt __far TimerHandler void clock_ticks 0 counter long PIT_FREQ frequency BIOSTimerHandler getvect TIMERINTR setvect TIMERINTR TimerHandler outp 0x43 0x34 outp 0x40 counter 256 outp 0x40 counter 256 set the com port mode and ISR function void SetComl void interrupt __far ComHandler void BIOSComlHandler getvect COMIINTR setvect COMLINTR ComHandler gps_port_intr_set inportb GPS_PORT_INTR outportb GPS_PORT_CONT 11 outportb GPS_PORT_INTR 1 inportb GPS_PORT_DATA outp 0x21 inportb 0x21 amp 0xEF reset timer rate and ISR function void CleanUpTimer void outp 0x43 0x34
128. py Pk Pkm1 8 8 vec xk xkm1 8 filter count t m fxkValid TRUE 119 Vita David McNeil Mayhew was born on September 15 1976 in Dale City Virginia He attended North Stafford High School and graduated in 1994 David entered Virginia Polytechnic Institute and State University as an undergraduate in the Engineering program in fall of 1994 David graduated with his Bachelor of Science in Computer Engineering in December of 1997 David then remained at Virginia Tech and completed his Masters of Science Degree in Electrical Engineering in the summer of 1999 David has taken an engineering position with Intelligent Automation Inc a robotics and artificial intelligence research and development company located in Rockville Maryland 120
129. r Resetting the mode to the text mode which we are used to is just as simple We just put 03 hex into the ax register and generate the same interrupt This is shown in the function set_text void set_text void asm pusha store the A register so we don t mess it up mov ax 0x0003 load 0x03 into the A register text mode int 0x10 call the BIOS interrupt to set mode 0x03 popa restore the A register After the mode has been initialized plotting a pixel is as simple as writing the color byte to the appropriate position in memory The video memory array starts at the upper left corner of the screen runs across the screen line by line and ends at the lower right corner of the screen LaMothe et al 1994 Increasing X index Increasing Y Figure 6 4 VGA screen memory orientation So we orient our coordinate system such that the x axis starts on the left and points right while our y axis starts at the top and points down Thus if we want to plot a point at the position we simply write to the array at offset y 320 x The 39 routine blit_bit plots a pixel of value color to the screen at position pointer to the screen memory is passed in vscreen which is always A0000 hex common way to calculate the offset into the array is to use bit shifts rather than a multiplication Note that y 320 is equal to 256 64 which may also be written as y lt lt 8 y lt lt 6 The rati
130. reen int i d1lx dly d2x d2y u s v m n xl yl x2 y2 xl int pl x starting x point yl int pl y starting y point x2 int p2 x stopping x point y2 int p2 y stopping y point u x2 x1 overall run of the line 2 1 overall rise of the line dlx sgn u line top to bottom or bottom to top dly sgn v line left to right or right to left d2x sgn line top to bottom or bottom to top d2y 0 m abs u n abs if m lt n if Slope lt 1 d2x 0 d2y sgn v m abs v n abs u 5 2 for 1 0 1 lt 1 plot single pixel the screen only if within map bounds if yl gt MBULY amp amp yl lt MBLRY amp amp gt MBULX amp amp lt MBLRX 102 vscreen y1 lt lt 8 y1 lt lt 6 x1 color s n increment to next pixel location if s gt m S m xl dlx yl else xl d2x 2 return the sign of int sgn int if a 0 return 1 if a 0 return 1 return 0 draw a solid colored box void fill box int sx int sy int width int height unsigned int color unsigned char far vscreen int offset i offset sy lt lt 8 sy lt lt 6 sx for i sy i lt sytheight i _fmemset Char vscreentoffset color width offset 320 fill the entire screen with a given color
131. ristics For this project a small amount of the road segment intersection map for downtown New York City was generated manually using data from a Geographic Information System GIS software package This was a very tedious and time consuming process which could be automated in future work One of two options could be used to make the process much easier and more general in future use The first option is to write a program that interfaces into a GIS database and outputs the intersection road segment format map for the map matching program to read The second option is to incorporate the capability to read the GIS database directly into the sensor fusion map 73 matching program second option is the more desirable one as it is more generalized and can be made to work any place in the world by simply using a different GIS database Another area of improvement for the project is the required hardware for the sensor collection and integration This project used a variety of sensors a large data acquisition board and a laptop computer For this type of solution to be viable in a consumer type product unneeded sensors should be removed along with the expensive data acquisition board The laptop computer could also be replaced with a cheaper embedded system provided a way to enter data into the system and to remove data from the system 8 2 Conclusion This project has shown that significant vehicle position estimate r
132. rminate and stay resident 6 2 2 Com Port Interrupt Programming Activating the ISR for com port programming is quite similar to activating the ISR for the system timer The functions SetCom1 and Cl1eanUpCom1 are analogous to SetTimer and CleanUpTimer described in the section above These functions are shown here define COMIINTR 0x0C com port ISR jump vector number define GPS_PORT_DATA 0 3 8 port where data arrives define GPS_PORT_STATUS Ox3FD line status register define GPS PORT INTR Ox3F9 interrupt enable register define GPS PORT CONT Ox3FC modem control register void SetComl void interrupt __far ComHandler void BIOSComlHandler getvect get old com ISR handler setvect COMIINTR ComHandler set new com ISR handler gps_port_intr_set inportb GPS_PORT_INTR get initial port settings outportb GPS_PORT_CONT 11 assert DTR and RTS signals outportb GPS_PORT_INTR 1 set to interrupt on data received inportb GPS_PORT_DATA clear any pending data on com port outp 0x21 inportb 0x21 amp 0xEF void CleanUpComl void outportb GPS PORT INTR gps port intr set return initial port settings setvect COMIINTR BIOSComlHandler set old com ISR handler Note that SetCom1 does more than just set the jump vector for the ISR It also must do some setup specifically for the com port Most importantly it sets the com port
133. rom file if fread amp log data sizeof T LOG DATA 1 10g file NULL done input from log file set flags running 0 if running draw sensor vals BACKGNDCOL vscreen draw over old sensor values sen val 0 21og data gyrol sen 1 1 109_ data gyro2 sen_val 2 log_data accelf sen_val 3 log_data accelr sen_val 4 log_data steer draw_sensor_vals SENSORCOL vscreen draw new sensor values old_data curr_old_data log_data store log data in array curr_old_datatt if curr old data OLD DATA SIZE curr_old_data 0 curr speed estimate speed from delta positions old data curr old data OLD DATA SIZE 1 O0LD DATA SIZE odometer total old data curr old data OLD DATA SIZE 1 SPEED DELTA OLD DATA SIZE odometer total SPEED FACT if logging if new gps whole gps report received Pos Status Data Decode gps report amp GPS chan 0 decode GPS message ps time ti hour GPS chan hours ps time ti min GPS chan minutes ps time ti sec floor GPS chan seconds ps time ti hund floor GPS chan seconds 100 0 ps time ti hund 100 our gps time ti hour min gps time ti min sec gps time ti sec gps seconds hour 3600 min 60 sec gps hsec gps time ti hund ps hsec gps seconds 100 gps valid 1 check receiver status if GPS chan rcvr status amp 0x43 gps valid 0 if GPS chan rcvr status amp 0x30 gps valid 0
134. rs to the process of updating the fusion parameters by hand This is the easiest method of optimization to implement because there is actually no implementation but it is also the most tedious method to use for the update process This method consists of trying a given set of parameters evaluating the results of that set and then using some method to update a particular parameter Typically the update method involves some sort of intuition about the nature of the system and the type of error evident in the output This method is often used to arrive at a crude starting point for another more precise method such as those listed below 5 6 2 Iterative Hill Climbing The iterative hill climbing optimization method is an algorithmic implementation of what manual optimization often involves Manual optimization has the benefit of human intuition in choosing parameter values but the iterative hill climbing algorithm has the benefit of being easily implemented on a computer Thus the iterative hill 42 climbing method be run automatically for an extended period of time producing result with relative ease In general the hill climbing algorithm starts with an initial guess about the parameter values of the optimum solution Then one of the parameters is changed by a suitably chosen or guessed increment If the evaluation function gets better we keep moving in the same direction by the same increment If the function gets wor
135. ructure Connections can be treated simply as an array of pointers to other nodes For general navigation after the initial position is known maintaining a position estimate is simply a matter of traversing the linked mesh of nodes For example suppose node A is connected to nodes B C and D shown in Figure 5 19 below Then if we estimate our current position to be near node A then we know that we only need to consider movement towards nodes C and D Figure 5 19 Mesh of nodes intersections map representation 48 Given that the algorithm needs consider only the adjacent intersections the problem is reduced to that of choosing the correct one Suppose that we are traveling from intersection C through intersection A and on to intersection B The vehicle s position estimate can be maintained from intersection C to intersection A using any combination of the previously mentioned techniques utilizing inertial and dead reckoning sensors and GPS data where available As the vehicle moves near intersection A the algorithm begins to consider which of the adjacent nodes will be visited next The vehicle s position estimate from intersection C to intersection A is performed by inertial dead reckoning and GPS sensors These sensors may be prone to drift errors such that there is an ambiguous region of radius near intersection A In this ambiguous region the map matching algorithm is not able to determine on which road s
136. ructure would occupy eight bytes 4 for the character and 4 for the long integer The character itself is only one of the four memory bytes but the compiler decided to position the 32 bit long integer evenly on a 4 byte chunk of memory Usually a programmer does not notice this difference when porting code from a 16 bit to a 32 bit platform In this case however a problem was introduced because of the cross use of data files written on one platform and read on the other When we wrote the data structures to disk we would call a command such as fwrite amp mydata sizeof mystructure 1 myfile where mydata is of type mystructure and myfile is a file stream pointer Then we would like to read the data in the Win95 program using a function call like myfile Read amp mydata sizeof mystructure 66 where myfile is an instantiation of the class CFile Since the sizeof operator returns the number of actual bytes of memory consumed by the structure we would write 5 bytes and read 8 bytes In addition the alignment within the structure would be off and the data would be garbage The solution to all of this turned out to be quite simple Using a preprocessor directive called pragma pack we are able to force MSVC to pack the structures to the 1 byte boundary The usage is as follows pragma pack push identifier 1 typedef struct _mystructure char a int pf mystructure pragma pack pop identifier where identif
137. s 2g full scale In this application sensor data is collected at 100Hz so filtering out frequencies above 1000Hz removes high frequency signal components which cannot be removed in software The sensor outputs approximately 2 to 5 mg thousandths of a gravity of noise in the frequency range which must be removed during sensor filtering Two accelerometers were employed on our sensor board One that measured longitudinal acceleration and one that measured lateral acceleration The accelerometers relative orientations are shown in Figure 3 5 Measures acceleration in the y direction longitudinal Measures acceleration in the x direction lateral Figure 3 5 Diagram of accelerometer orientations on sensor board From this data we can integrate to find relative speed and heading This is discussed in more detail in the chapter on system modeling An example plot of the inertial sensor outputs for a 10 second time frame is shown in Figure 3 6 Inertial Sensor Outputs vs Time 3500 2 Fwd Rev 3300 9100 amp Steering P jometer 2900 Side Accelerometer o T 2700 r o Vehicle turning Vehicle decelerating 2500 T T T T T T T T T T T T T T T T 1 1 101 201 301 401 501 601 701 801 901 1001 Time hundreths of seconds Figure 3 6 Example plot of inertial sensor outputs 3 4 Data Acquisition Board The data acquisition board we used on t
138. se we undo the last increment and start changing one of the other parameters This process continues through all of the coordinates until all the coordinates have been tested We then halve the increment amount reverse its sign and start again with the newest set of parameters The entire process continues until the increments have been halved enough times that the parameters have been determined with the desired accuracy Horst et al 1995 For example supposing we had only two parameter values to optimize we might end of with the sequence of parameter values indicated in Figure 5 11 until a suitable goal is achieved Parameter 2 Parameter 1 Figure 5 11 Iterative hill climbing optimization method 5 6 3 Genetic Algorithms Note that in both the manual and the hill climbing optimization techniques the parameter optimization is easily subject to getting stuck in local maxima or local minima when minimizing the fitness function This is a problem for many optimization problems and genetic algorithms have often been used to generate more optimal solutions than those that may be obtained by standard hill climbing or gradient descent methods 43 Genetic algorithms apply the rules of reproduction gene crossover and mutation to pseudo organisms so those organisms can pass beneficial and survival enhancing traits to new generations In our case the pseudo organisms would represent sets of sensor fusion parameters Just
139. spection and comparison of the generated outputs To effectively fine tune the system and optimize the numerous fusion parameters a more precise evaluation system must be implemented This is not an easy task because it requires knowledge of the desired output to which the actual system output must be compared Performing this analysis task could be aided by the use of the map matching techniques discussed earlier in this thesis However evaluating the output from the map matching system would still require the beforehand knowledge of the actual path traversed by the vehicle Ideally the process of evaluating the entire system would proceed as follows 1 Generate a intersection road segment based map for a well known region such as downtown New York City as discussed in the map matching section of this thesis This could be done using a GIS package such as ArcInfo or Etak Alternately this map could be generated by collecting near perfect GPS data on the roadway This is usually done by collecting samples in one location over a long period of time and averaging the data 2 Collect a large amount of inertial sensor and GPS data within that region 3 Store the list of road segments traversed or the list of intersections traversed while collecting the GPS and inertial sensor data 72 4 Post process the GPS and inertial sensor data using only the Kalman filter and sensor fusion algorithms as discussed earlier in t
140. sy int width int height unsigned char box unsigned char far vscreen int vsoff i j boxoff boxoff 0 vsoff sy lt lt 6 sy lt lt 8 sx for i 0 i lt height i for 3 0 j lt width j box boxoff vscreen vsofft j vsoff 320 new GPS data collected draw new values and indicators on the screen void new_gps_stuff void int i draw_strength_vals BACKGNDCOL vscreen for 1 0 1 lt 6 1 strength i GPS_chan channel i strength draw_strength_vals YELLOW vscreen if old_gps_valid gps_valid if gps_valid fill_box GPSVALPOSX GPSVALPOSY GPSVALWIDTH GPSVALHEIGHT GREEN vscreen else fill_box GPSVALPOSX GPSVALPOSY GPSVALWIDTH GPSVALHEIGHT RED vscreen old_gps_valid gps_valid if old_sats_tracked GPS_chan sats_tracked blit_char SATTRKPOSX SATTRKPOSY old_sats_tracked 0 BACKGNDCOL vscreen old_sats_tracked GPS_chan sats_tracked blit_char SATTRKPOSX SATTRKPOSY old_sats_tracked 0 TEXTCOL vscreen convert a lat long point to screen coordinates POINT 11 to screen POINT 11 int offx offy POINT retval offx int float MBLRX MBULX offy int float MBLRY MBULY retval x MBULX offx retval y MBULYt offy return retval float maplrlong mapullong float llp x mapullong float mapullat maplrlat float mapullat llp y 108 return 1 if point p is in the map view else return 0 int in view POINT p
141. t increment global 100th second count new_sense 1 100th second elapsed set global flag clock_ticks counter if clock_ticks gt 0x10000L roughly 1 18 2 sec has elapsed clock ticks 0x10000L BIOSTimerHandler call the system timer handler else outp 0x20 0x20 clear interrupt and continue The keywords interrupt and far tell the compiler explicitly how to handle the function The keyword interrupt tells the compiler that this function is meant to be called as an ISR because the compiler must add special assembly calls to properly 54 return from an ISR keyword __far tells the compiler that this function may be called from outside of the current code segment These keywords are specific to Borland C version 4 5 and may need to be changed for other compilers Following are the code segments that initialize and clean up the system timer ISR along with the global declaration for the variable BIOSTimerHandler which is a pointer to hold the address of the system timer ISR Set Timer is called at the beginning of the program to setup the PIT chip and new timer ISR jump vector and CleanUpTimer is called to reset the PIT chip and old timer ISR jump vector A jump vector refers to a place in memory that holds the address of an ISR Every time any interrupt is generated the system looks at a predefined place in memory to find the address of the ISR or jump vector define TIMERINTR 0x0
142. t calculate and m 1 g df Ei dg 14 amp amp Jor xe f g 41 15 1 df a coso dg 54 sin 2 e 25 cosodo 16 C e sino cosodo 17 e sin Putting equations 15 16 17 into equation 14 get C e sin cose Jal e sin o JC a sino a cose V e sin lt 18 do l e sin 9 oe ear 2 1 Jl 2 sin yl e sin o l e sin o 83 1 e sin X a sino a J41 e 1 sin sin sino sing cos o yll sin a sing l e 1 cos 6 e cos 6 Jll a sing l e e cos e cos e sin e Finally we reduce to dx 1 19 de sin forz f a l e sing g l e sin 9 4 20 C e sino cosodo e sin e df 1 e cosodo dg Again combining equations 20 and 21 into equation 14 1 mm L e sin o 2 a 1 a 1 p2 e sin a e sino 222 22 de lI e sin 1 1 ELE e sin o 2 L e sin 2 coso e sin cose 1 1 L e sin e 1 sin 9 2 1 0 1 e coso e sin e 20 3 1 e sin o 2 Which reduces to _ 1 e cos 23 3 1
143. ter IEEE AES Systems Magazine January 1996 77 Appendix A GPS Basics A 1 History The immediate predecessor of today s modern GPS is the Navy Navigation Satellite System NNSS also called TRANSIT system This system included six satellites orbiting at altitudes of about 100 km with nearly circular polar orbits This system was developed by the US military to determine the coordinates of vessels and aircraft Civilian use of this satellite system was eventually authorized and the system became widely used worldwide both for navigation and surveying Some of the early TRANSIT experiments showed that accuracy of about one meter could be obtained by occupying a point for several days The Global Positioning System GPS was developed to replace the TRANSIT system because of two major shortcomings in the system Large time gaps in coverage were the main problem with TRANSIT Since a satellite would typically pass overhead every 90 minutes users had to interpolate their position between passes The second problem was its relatively low navigation accuracy The GPS system was designed to address these problems answering the questions what time what position and what velocity quickly accurately and inexpensively anywhere on the globe at any time Hofmann Wellenhof et al 1997 A 2 Overview The basic principle of GPS relies on the ability to measure distances from several points in space and performing triangulation based on th
144. to speed of the shaft on which it is mounted In an automobile this shaft must be located on the drive side of the transmission as opposed to the engine side This is because a shaft on the drive side of the transmission will always be spinning at a rate proportional to the speed of the vehicle Tachometers are generally cheaper than encoders are but they also have drawbacks that are more significant Because a tachometer outputs a voltage level the navigation scheme and subsequent filtering algorithm must take into account the possibility of variations in offset and magnitude due to slight differences in manufacturing from unit to unit Additionally environmental changes can alter the operating parameters of such a device somewhat In addition to get distance from a tachometer output the algorithm must integrate the signal over time This causes the small errors in the output signal to accumulate 17 4 2 3 Accelerometers Accelerometers can be used for both speed heading estimation Accelerometers provide a direct measurement of acceleration or force in one direction An accelerometer typically measures the acceleration of the car due to forces acting on the car Ideally by using only the two accelerometers one may determine 2 dimensional position at any given time However several problems arise when trying to do this Accelerometers usually measure more than what we actually want to measure The output signal is composed o
145. ults while also being easier to implement in the vehicle We used a signal from the anti lock brake ABS on the rear left wheel of the test vehicle The anti lock braking system uses a more accurate sensor for detecting wheel rotation as compared to the odometer which outputs a series of digital pulses These pulses can be picked off a wire that is part of the system and input directly into a data acquisition pulse counter In our vehicle a wire directly under the left rear seat carries this signal so it was simply a matter of splicing our leads into the wires already provided This is shown in Figure 3 3 This signal was already well suited for input into our data acquisition board and the output was accurate even for low speeds Each pulse from the ABS sensor indicates approximately 1 26 of a meter traveled or 0 03846 meters pulse This number can change slightly on a daily basis due to changes in tire pressure or road conditions ABS sensor signal connection into existing cables a a EU um m 2 Figure 3 3 ABS sensor connection found under the left rear seat Another sensor attached directly to the hardware of the vehicle was a pull string potentiometer wrapped around the steering column located under the dash in front of the driver This provided a direct measurement of the angle of the steering wheel which corresponds directly to the angle of the vehicle s front tires The potentiometer was used as a simple vo
146. ut value which indicates zero angular velocity and a conversion ratio a number which converts millivolts of sensor output to degrees sec These values vary as the physical properties of the gyroscope change due to environmental changes The methods by which we make these adjustments will be covered in section 5 6 Each time that new inertial data is received in the system at approximately 100Hz the Kalman filter is run to produce an optimal state estimate given the measurement information During each iteration updating the filter is accomplished in four main steps These are Prediction of covariance matrix of states A k 1 P k 1 k 2 1 Q 1 Calculation of Kalman gain matrix K k CP k C Update of the state estimation X k 2 k 1 KGlv k CA c 1 k 1 Update of covariance matrix of states P k K k CP 1 where P k is the predicted error covariance matrix at time k K k is the Kalman gain matrix at time k and P k is the updated error covariance matrix at time k R is the measurement noise matrix which contains the expected covariance of the measurement data Q is the state noise matrix which contains the expected covariance of the state matrix values Essentially R is a measure of confidence in the measurements and Q is a measure of confidence in the state estimate That is if an extremely precise instrument 22 were being used to quantify a p
147. vscreen else printf s n status_line SetTimer Handler 100 set timer interrupt to 100 Hz SetComl1 ComlHandler set com interrupt handler for user_key 0 user_key g amp amp user_key G get_user_key wait for 76 clear old message string and draw a new one if novga blit_string STATUSX STATUSY status_line BACKGNDCOL vscreen if logging sprintf status_line Logging s S to stop log file name else sprintf status line Reading s S to stop log file name if novga blit string STATUSX STATUSY status line TEXTCOL vscreen else printf sWMn status line main loop for data collection running 1 95 while running loop until running 0 while new_sense amp amp quickview wait until 01 sec if not quickviewing 01 second elapsed collect new sensor data new_sense 0 reset flag if start_hsec start_hsec hsec if logging logging acquire data from daqbook ifndef NOBOOK get odometer count daqAdcRd 0 amp log_data gyrol DgainX1 daqAdcRd 1 amp 1 data gyro2 DgainX1 daqAdcRd 2 amp log_data accelf DgainXl1 daqAdcRd 3 amp log_data accelr DgainXl1 daqAdcRd 4 amp log_data steer DgainX1 dendif log data ticks hsec log data odometer total odometer total fwrite amp log data sizeof T LOG DATA 1 1og file write to file post view read data f
148. wing two relations for x and ze x a a V1 e sin 1 By differentiating each of the equations we get dx a sin dz a 1 e cos Bdg 2 Therefore it follows that ds C dx dz 1 e cos p dp 3 and ung x ds 1 e cos dz 5 ds 1 e cos By multiplying Eq 4 by N1 and squaring Eq 5 we get 1 e sin 1 e Jsin o l 1 e cos p 2 2 1 e os p 7 1 e cos By adding Eq 6 and Eq 7 and taking the square root of each side 1 2 J1 e cos p 8 iee sin By using equations 4 and 5 with equation 8 we get gt sin sing 41 e cos 9 sin 1 2 2 cos 41 e cos coso 10 1 2 J1 e sin By combining equations 9 and 10 with equations 1 82 COS x 11 41 e sin a 1 e ice sin Re 12 After the rectangular coordinates of the point on the surface of the earth have been calculated we are now able to calculate the values and C2 The following calculations based on the above formulae were performed by Gene Felis Electronics Engineer NUWC Div Keyport in 1976 The constant C is the derivative of the ellipse at the position X Zc 2 C i ds _ 13 do do do dx Therefore we must firs
149. with the issues relating to each Additionally a detailed explanation of the Kalman filtering and rule based sensor fusion is given PC based software programming for actually implementing the system is discussed in detail as well 1 2 Scope and Structure of Thesis Chapter 2 provides a survey of current literature related to the topics of inertial navigation systems and algorithms GPS systems and other methods of sensor fusion in similar applications Chapter 3 covers all of the hardware that was used in this sensor fusion project This includes the test vehicle the data acquisition hardware and the means by which the acquisition hardware was interfaced to the vehicle hardware Additionally an explanation of some of the software commands that are specific to the selected hardware is presented Chapter 4 provides a comprehensive list of various sensors and sensor configurations that may be used in a sensor fusion application similar to the one presented in this thesis The dynamic equations that govern the system for each basic configuration are also covered Chapter 5 approaches the more advanced subject of filtering the inertial sensor outputs by means of a Kalman filter The specific filter for the configuration used in this project is presented which may easily be modified for other configurations Also the details about the rule based sensor fusion process and the reasoning behind it is given Several methods for sensor fusion p
150. xkm1 2 m_LastGoodGPSLong xkm1 1 m_LastGoodGPSDLat 0 m_LastGoodGPSDLong 0 m_NumGoodNoWeight 0 else if m_NumGoodNoWeight gt m_GPSNoUpdateThresh GPS data has been valid for some time but is far enough away such that is considered inaccurate double LastGoodGPSGPSDist SQR m GPSLongitude m LastGoodGPSLong SOR m GPSLatitude m LastGoodGPSLat double LastGoodGPSOdoDist SOR m LastGoodGPSDLat SOR m LastGoodGPSDLong if LastGoodGPSGPSDist 1 25 LastGoodGPSOdoDist m NumGoodNoWeight m GPSNoUpdateThresh2 we think we messed up reinitialize position and heading as if new beginning double newhead 90 0 m GPSHeading newhead fmod newhead 360 0 360 0 xkm1 1 m GPSLongitude xkm1 2 m GPSLatitude xkm1 3 m_NumGoodNoWeight 0 else GPS data is invalid so ignore it altogeter plot the filtered position in the output window if m fInitGPS amp amp m fxkValid m_pMapDlg gt PlotFiltPos CPoint int xk 1 int xk 2 115 FuzzylHz is called to perform the fuzzy based sensor fusion when new GPS data is received Either FilterlHz or FuzzylHz is called depending on the user selection void CPostViewApp FuzzylHz void we just received new GPS data and wish to Fuzzy fuse GPS and sensor data roughly 1 Hz if m fInitGPS amp amp m GPSGood m_fInitGPS TRUE just got first good GPS value gt initialize position et
151. y complex and the details of implementation are beyond the scope of this thesis In order to get overlapped I O working well we ended up converting some of Microsoft s example code to suit our purpose It would have been very difficult otherwise If you are trying to figure out overlapped I O for 65 Windows 95 NT we suggest referring to the Microsoft examples our code in the appendix to this thesis 6 3 4 File I O File I O under Win95 using the MFC is very similar to old fashioned C style library functions such as fread and fwrite Using the CFile class from the MFC provides the member functions Read and Write for performing binary file reads and writes almost exactly like fread and fwrite The problem for us occurred when we tried to take data collected from our DOS program and read it into our Win95 program The problem has to do the packing alignment for our data structures Packing alignment refers to the alignment of structure data members in memory Under the DOS programming environment compilers typically pack data members on the byte one byte boundary Under a 32 bit OS such as Win95 or UNIX compilers typically pack data members on the 4 byte boundary For example note the following structure struct mystructure long Using a typical DOS compiler this structure would occupy five bytes of memory 1 byte for the character and 4 for the long integer Using Win95 compiler however this st
152. z That is our Kalman filtered state vector is not directly dependent on the GPS receiver output Instead we use some sort of fusion algorithm to combine the output from the Kalman filtered inertial data and the GPS data Inertial GPS sensor fusion amounts to dynamically determining weights with which we combine the position determined by the INS data and the position output from the GPS receiver Thus we would like to determine the best values for Y and P for the weightings of GPS position and heading respectively In our case we do not determine a value to fuse GPS and INS velocities because we assume that the INS velocity is the best measurement available to the system After determining the weights we fuse the inertial and GPS data using the following equations Lat Vos Latgps 1 Lat ns Long Epos LON cps 1 Long ins Heading V Heading cps 1 Y prap Heading ys where Lat Long and Heading are the estimated latitude longitude and heading respectively Note that these weights will be 1 when initializing the system and they with be 0 when GPS data is bad When the GPS data is good the weights will lie somewhere in the range 0 1 5 2 Geodetic to Ground Conversion In order to relate the geodetic Observer coordinates latitude and longitude to local ground coordinates X and Y in meters a conversion equation is Equatorial plane s 7 Figure 5 4 Cross

Multi-rate Sensor Fusion for GPS Navigation Using Kalman Filte

Contents

Download Pdf Manuals

Related Search

Related Contents