Home

fulltext - DiVA Portal

1. 50 Acc y m s Acc x m s Acc z m s CHAPTER 5 RESULTS 20 20 0 5 10 15 2 25 30 35 40 45 50 55 Time s 20 0 20 0 5 10 15 2 525 30 35 4 2450 50 55 Time s 50 0 50 0 5 10 15 20 25 30 35 40 45 50 5 Time s Figure 5 6 Dynamic acceleration estimate with GPS coverage Bias m s 0 8 0 6 A 0 4 0 2 aea 10 20 30 40 50 60 Time s Figure 5 7 Bias estimate during dynamic conditions 5 2 DYNAMIC TESTS 51 300 250 200 150 100 Yaw angle deg 50 100 0 10 20 30 40 50 60 Time s Figure 5 8 Measured yaw angle during dynamic conditions Estimated 6P5 Time s Estimated GPS 0 5 10 15 20 25 30 35 40 45 50 55 Time s 200 Estimated GPS 5 10 15 20 25 30 5 AO 45 050 156 Time s Figure 5 9 Dynamic postion estimate with GPS dropout 92 CHAPTER 5 RESULTS Due to uncertainties on true position it is hard to calculate the RMS cor rectly By wrongfully using the GPS measurements as the correct position the RMS values for the deviation can be calculated The results are shown in Table 5 2 As can be seen the position RMS is actually lower in north east direction than it was during static conditions This is merely due to the subtraction of the GPS position North East Down Velocity RMS 2 0428 1 4548 0 6307 Position RMS 2 1724 1 3248 0 3350 Table 5 2 RMS values integr
2. start x for i 1 size a x prevX x dt i 0 1 prevtime alltime Rb2t updateR roll i pitch i yaw i accel Rb2t a x i a y i a z i biasAcc O O 9 9506 ci 0 5 c2 0 5 c3 1 0 bi 1 6 b4 1 6 b2 1 3 b3 1 3 a2l 0 5 a32 0 5 74 APPENDIX B SOURCE CODE a43 1 0 hif ki k2 IMU_f x prevAccel WGS_a WGS_e IMU f x dt i a21 k1 1 ci prevAccel ci accel WGS a WGS e k3 IMU f x dt i a32 k2 1 c2 prevAccel c2 accel WGS a WGS e k4 IMU f x dt i a43 k3 1 c3 prevAccel c3 accel WGS a WGS e x dt i b1i k1 b2 k2 b3 k3 b4 k4 prevAccel accel totalTime totalTime dt i alltime alltime dt i totatt totatt roll i pitch i yaw i numit numit 1 false if count lt numel gps_data time if totalTime gt gps data time count gps data time 1 amp amp count lt 40 totalTime gt 60 count2 count2 1 if totalTime gt 60 amp amp count lt 50 count 60 end R_phi WGS a 1 WGS e 2 1 YGS e sin x 1 1 2 1 5 R lambda WGS_a sqrt 1 WGS_e sin x 1 1 2 Rt2E 1 R phi x 3 1 1 R lambda x 3 1 cos x 1 1 1 Transferring the offset to ECEF tmpoff diag Rt2E Rb2t posoffs Only used for calculating RMS values gps_speed gps data S0G count cosd gps data COG count sind gps_data COG count 0 tmp count gps_speed x 4 6 x tilde gps data lat count g
3. NTNU Norwegian University of Science and Technology Development of a Low Cost Integrated Navigation System for USVs Haakon Ellingsen Master of Science in Engineering Cybernetics Submission date August 2008 Supervisor Morten Breivik ITK Norwegian University of Science and Technology Department of Engineering Cybernetics Problem Description 1 Set up and present a progress plan for your thesis work so that the due date can be met 2 Review state of the art integrated navigation systems for marine applications with respect to criteria such as performance and price 3 Acquire commercial off the shelf GPS and IMU units e g u blox and MicroStrain products and co locate these units in a suitable container 4 Develop sensor fusion algorithms for integration of GPS and IMU signals Verify and validate your design by numerical simulations in Matlab Simulink 5 Verify and validate your design by full scale experiments with a USV Assignment given 14 January 2008 Supervisor Morten Breivik ITK Abstract This report considers the real time implementation approach of an integra tion between an Inertial Navigation System INS and a Global Positioning System GPS The integration has been performed using a GlobalSat EM 411 GPS receiver and a Microstrain 3DM GX1 Inertial Measurement Unit IMU This has been performed by incorporating a Kalman filter and aiding the INS estimates through GPS measurements The goal o
4. 1 axis auto y figure 4 subplot 311 plot est s 4 est s 1 4 est_s 1 subplot 312 plot est s 4 est s 1 4 est_s 2 subplot 313 plot est s 4 est s 1 4 est s 3 start 35 Calculating RMS values p n sqrt data start numel data 2 2 data start numel data 2 start 1 p e sqrt data start numel data 3 3 data start numel data 3 start 1 p d sqrt data start numel data 4 4 data start numel data 4 start 1 v n sqrt data start numel data 5 5 data start numel data 5 start 1 v e sqrt data start numel data 6 6 data start numel data 6 start 1 v d sqrt data start numel data 7 7 data start numel data 7 start 1 numel data numel data numel data numel data numel data numel data 77 12224297 492499 f 244 44 5 5 402 4627 OI 78 APPENDIX B SOURCE CODE a x sqrt accel2 1 accel2 1 numel accel2 1 sqrt accel2 2 accel2 2 numel accel2 2 sqrt accel2 3 accel2 3 numel accel2 3 w N lt oul figure 5 plot biascount legend X Y Z xlabel C Time s ylabel C Bias m s 2 B 1 9 Rt2eg m function R Rt2eg Phi lambda h a 6378137 e 8 1819190842622E 2 R_phi a 1 e 2 sqrt 1 e 2 sind Phi 2 R_lambda a sqrt 1 e 2 sind Phi 2 R 1 R_phith 0 0 O 1 R_lambda
5. 1 data 3 APPENDIX B SOURCE CODE plot gps_data time 1 numel gpspos 3 gps_data time 1 gpspos 2 r legend Estimated GPS plot 0 70 0 b axis 0 55 1 1 axis auto y subplot 3 1 3 hold on plot data 1 data 4 plot gps_data time 1 numel gpspos 3 gps_data time 1 gpspos 3 r legend Estimated GPS axis 0 55 1 1 axis auto y figure 2 subplot 3 1 1 hold on plot 0 70 0 b plot data 1 data 5 xlabel Time s ylabel Speed N m s axis 0 55 1 1 axis auto y subplot 3 1 2 hold on plot 0 70 0 b plot data 1 data 6 xlabel Time s ylabel Speed E m s axis 0 55 10 25 axis auto y subplot 3 1 3 hold on plot 0 70 0 b plot data 1 data 7 xlabel Time s ylabel Speed D m s axis 0 55 1 2 axis auto y figure 3 subplot 3 1 1 hold on plot 0 70 0 b B 1 MATLAB plot data 1 accel2 1 xlabel Time s ylabelC Acc x m s 2 axis 0 55 1 1 axis auto y Subplot 3 1 2 hold on 4 plot 0 70 0 b plot data 1 accel2 2 xlabel Time s ylabel Acc y m s 2 axis 0 55 1 1 axis auto y subplot 3 1 3 hold on plot 0 70 0 b plot data 1 accel2 3 xlabel Time s ylabel C Acc z m s 2 axis 0 55 1
6. 2wje cos A ug AUN codes 0 0 E Eg t cud 1 t p AS R v 0 Re h cos 0 vV 4 8 0 0 1 UThis applies only for the boldfaced notation 4 2 GPS POSITION AIDED INS 39 The bias calculation will be discussed later in this chapter The relationship between the measured and true output from the INS can be written as f f f 4 9 Wy wh Ob 4 10 where the the error equation is given as 9 f Af db nl Va 4 11 wi IA wy 0by onl v 4 12 These equations are also known as the truth model where e b is a random walk variable e g db v where P v 0 e dnl represents all the non linear errors e v is the measurement noise e dA is due to misalignment of the IMU The misalignment matrix is given as SF 00uw Au A day SP a 00ww Am OS E 051 dus Ds QUAL S agr OS 000 0 g dry OOF and nl is the nonlinear error given as bab T krof KUE kra f2 T kes fufv T kes fo fw T keo fw fz dnl bu Zum kyo f2 n keys fi D katate ng kys fo fw dic keto fa kaf i kaf xn kz f2 a kz fufv EN kas fofw E Esfus ado SE kp f T kp fe Tv hea Jadu gg ne Fed a T kpo fw fu nl kaf J kat Tv kgs f EH kgs tude S kas fo fw E Fsofudu ka Lo ER krof T kraf E kr fufv T krs fo fw T kro fw fu 40 CHAPTER 4 INTEGRATING GPS AND INS Since the truth model gives a lot of parameters to be estimated both the alingment err
7. 3 3 Hardware Selection aus caos sera med Be oe eh Roda eec Sund 30 Segal MU p an e io doth Ree ew e ET 30 3 3 2 GPS Receiver 5 diga emm oh bed o eed 31 3 3 3 NMEN USS Sed ap sane ta ox 32 Integrating GPS and INS 35 4 1 A Dr uos Sob dc Sob Rod Sc eed 35 4 1 1 Direet Filterine ou e Gate ee SO ses 35 4 1 2 Complementary Filtering 36 4 2 GPS Position Aided INS 2 2 2222 02002000 38 KIN ME je Bn ME oh cle ed Dc 40 43 Kalman Filter usur Se ses Kis Fan et ok a da 40 4 3 1 Discrete Kalman Filter 41 4 3 2 Integration Kalman Equations 2 2 2 2 2 22 20 42 4 4 Equations Summary ae E 43 Dp PMU o uat tiic ran Send 43 AAP GESA aro E CCP 43 4 4 3 Kalman Filtering A E sees a 43 Results 45 5 1 Stationary Tests moenia ar ada E oe or d 46 52 Dynamo Tests sakset Dee a Gas 48 Discussion and Summary 57 bibe ASUS FE AAA A A AO GSE ee 57 52 Future Work AN ote e ORO Hee o de 58 Data Sheets 59 A 1 Microstrain 3 DM GX1 reete e E e We aeg 59 A 2 GlobalSat EM All zzi ku ee A 61 Source Code 63 Bet Matla D atit aet ee Gl eee RES E eet Sok aon Ge des 63 Balal A ee ee ee 63 B 1 2 gpsreader m av me ee qe hd 63 Bl imuopenm La ad oe SE US 65 CONTENTS VII BLA imureader m sau et och Ders Pre 66 BLE IMU AA a er ee 69 BELO SSI sense anal 70 B T kalman ti Desse ne ee 71 Ble HODE sa A V nb de e erp e RU n UD tC 71 DI GNDAOS DER ee bed 78 B 1 10 sysinit m ooa the iw a oft vaste
8. 40 50 60 70 Time s T 1 E ij sg Ed e o a e 1 10 20 3 40 50 60 70 Time s 0 1 0 0 1 0 10 20 3 40 50 60 70 Time s Figure 5 2 Stationary speed deviation with GPS dropout Acc y m s Acc x m s Acc z m s 0 2 0 2 Time s 0 5 0 5 0 Time s a ho e 0 2 0 10 20 3 40 50 60 70 Time s Figure 5 3 Stationary acceleration deviation with GPS dropout 48 CHAPTER 5 RESULTS Table 5 1 shows the root mean square values of the deviation in acceleration speed and position with the GPS being online North East Down Acceleration RMS 0 05 0 02 Velocity RMS 0 09 0 01 Position RMS 3 0 005 Table 5 1 RMS values integrated INS GPS in static conditions Calculating RMS values during GPS dropout would be pointless as this will increase exponentially over time Comparing Table 5 1 with Figure 1 1 it is clear to see that the low cost application range within the specifications of the Crossbow NAV420 and is about twice to that of the Kongsberg Seatex Seapath 100 while being stationary meaning that the success of the low cost application is feasible Tt is worth noting however that the down component has such low Root Mean Square readings due to the altitude of the USV being assumed 0 whenever GPS measurements arrive 5 2 Dynamic Tests Next it s time to test the system in motion As no proper system were availiable for benchmarking a previous
9. Frames In navigation several reference frames can be used to present the data De pending on what navigational system is used to obtain the measurements different reference systems are usually required 2 1 1 Inertial Frame For the inertial frame Newton s law s of motion applies This means that the frame itself can not accelerate but is either stationary or travels with constant speed Its origin can be chosen anywhere 2 1 2 Earth Centered Earth Fixed The Earth Centered Earth Fixed ECEF frame has as the name suggests its center in the center of the Earth and the frame is stationary relative to the surface Of all the possible combinations of ECEF coordinate systems two are of particular importance The first representation frame gives its position in cartesian coordinates based on its distance from the center according to each axis This is named the ECEF rectangular system but is usually just referred to as the ECEF T 8 CHAPTER 2 REFERENCE FRAMES AND TRANSFORMATIONS Figure 2 1 The Earth with both ECEF frames and the local geodetic frame system Its x axis points through the intersection of the prime median 0 longitude and equator 0 latitude its z axis towards the true north pole and the y axis to complete the right hand rule through the intersection of 90 longitude and equator The other representation is called ECEF geodetic frame This system ex presses position in latitude longitude a
10. This also applies for the Earth s atmosphere and particularly the ionosphere However as the speed of signals in this layer is dependent on frequency a two frequency receiver can estimate this error easily For a single frequency receiver the estimate is dependent on atmospheric modelling The different layers of the atmosphere can be viewed in Figure 3 2 The ionosphere is the upmost part of the atmosphere and is ionized by solar radiation From the display given in Figure 3 2 it is located within the Thermosphere As atmospheric errors are dependent on the distance the signal travels through the atmosphere they are smallest when the satellite is positioned straight 6 This phenomenon is also known as dispersion 26 CHAPTER 3 SYSTEM CONSEPT above the receiver The more correct the estimated position is the more accurately the atmospheric error can be calculated Another atmospheric error is the tropospheric delay which is dependent on atmospheric pressure temperature humidity and satellite elevation This delay is usually separated into a wet and a dry component where the dry component contributes to approximately 90 of the delay This component is relatively well modelled while the wet one is more complicated due to many local factors There are several different models for the tropospheric delay which are good or bad depending on the angle of the satellite As opposed to the ionospheric error the tropospheric delay is
11. Two or more GPS receivers placed on two previously known locations used to estimate the heading or other Euler angles 15 16 CHAPTER 3 SYSTEM CONSEPT 3 1 1 Accelerometers An ideal accelerometer can be viewed as an ideal mass spring damper system where the position p relative to the casing is assumed perfectly measured 9 Then M k b P DE b 3 1 where k is the spring constant b is the damping constant and m is the mass p denotes the positional displacement from the equilibrium The force measured from the accelerometers is defined as f p 3 2 Should the system also be affected by gravity the equation becomes k b PE PE Drei 3 3 where g p denotes the position relative gravity By comparing with 3 1 and 3 2 it is trivial to see that f p glp 3 4 In the case of the accelerometers being within the earth s field of gravity and rotating around the earth s rate of rotation i e resting on the surface of the earth the equation is adjusted into k b 0 p p g p Nu NP 3 5 m m where Q is the skew symmetric form of the earth s rate of rotation vector This equation shows that in case of the accelerometer being stationary rela tive to the earth s surface f g N 0 p When operating in free fal the accelerometers will read 0 This means that the user needs to compensate for the forces of gravity when operating an accelerometer For the future th
12. also able to sustain long periods without GPS aid but whenever in motion of basically any kind it starts to drift fast even capable of obtaining several hundred meters devi ation in just 20 seconds With this kind of performance it would be pointless to rely on the IMU delivering decent position estimates during GPS dropout It could deliver satisfactory results when the vessel is subject only to small accelerations but on a USV this is hardly ever the case However when reviewing the deviation statistics and data plots from the previous chapter with the specifications of the Crossbow NAV420CA 100 a comparison can be made As no drift characteristics are given it is hard to draw any conclusions The crossbow has a circular error of propability of 3 which actually is higher than using differential GPS which usually operates at around 5 RMS The Crossbow velocity accuracy however is about a third of the average RMS obtained in dynamic conditions implying the IMU is of a higher standard which can also be seen directly from the acceleration accuracy on the IMU datasheet listed in Appendix A So if any conclusion is to be made the IMU is clearly not performing at such a level that any successful integration can be made at least not for a USV The application works satisfactory whenever GPS coverage is availiable but during dropouts the IMU starts to drift fast and uncontrolled The solution Iwhen assuming normal distribution a CEP o
13. and speed measurements without differ entiation and is thus less susceptible to noise e Integration provides real time estimates as opposed to differentation e The GPS corrects the integration error from a stand alone INS system e The GPS allows on line calibration of IMU errors and alignment of the IMU platform In other words by integrating the measurements position and speed can be obtained at the same bandwith as the measurement unit 1 2 BACKGROUND 3 1 2 1 Survey of Existing Solutions As of this date several integrated solutions between GPS and INS already exists but are in general known to be relatively expensive For use in low cost applications like for asmall Unmanned Surface Vehicle USV acommercial existing integrated solution can easilly exceed the price of the USV itself Thus it is desirable to develop a low cost integrated solution between a GPS and an INS using low cost components in order to keep the total cost down One state of the art integrated navigation system is the Kongsberg Manufacturer Crossbow Kongsberg Kongsberg Name NAV420CA 100 Seatex Seapath 100 Seatex Seapath 200 Perfomance Position Accuracy m rms 3 CEP 3 CEP 3 CEP 1 5 1 5 0 05 5 0 7 0 7 0 05 5 Attitude Accuracy deg rms 0 75 2 5 0 75 2 5 3 0 05 0 05 0 2 0 02 0 02 0 075 Velocity Accuracy m s rms 0 4 0 4 0 5 0 05 0 05 0 03 0 03 Angular Rate Accuracy deg s rms 0
14. case results This is due to some of the tests results being result of pure luck leading to a better performance then what can really be expected 45 46 CHAPTER 5 RESULTS 5 1 Stationary Tests D 10 20 30 40 50 60 70 Time s 10 Estimated e o tL 0 10 20 30 40 50 60 70 Time s 0 10 20 30 40 50 60 70 Time s Figure 5 1 Stationary position wih GPS dropout For preliminary tests both the IMU and the GPS receiver were kept station ary and the measurements were logged in Matlab By using these measure ments as input into the model found in the previous chapter the equations were tested to determine any errors At 40 to 60 seconds the GPS discon nects This process were not done in real time solving another problem hence the lack of proper computational hardware As can be seen from Figure 5 3 the acceleration estimates manage to converge towards 0 resulting in a maximum deviation of less than 10 meters after 40 seconds without GPS aid in the east direction the result shown in Figure 5 1 Although these figures shows promising results they should not be taken into to high regard however as the system is stationary However they do show that the simplifications of the IMU equations is close to that of the truth model and that the system performs satisfactory so far as long as the GPS measurements are credible 5 1 STATIONARY TESTS 47 Speed N m s Speed D m s 0 5 0 0 5 0 10 20 30
15. estimate of the states Proper derivation of the filter equations can be found in 9 10 or several other sources 4 3 2 Integration Kalman Equations The Kalman filter input y is already given in 4 15 This can be used together with the knowledge of the error of the GPS and the INS data to derive the state space error equations As the Globalsat EM 411 fails to deliver sudo ranges the GPS error is as sumed only to be white noise although not correct For the IMU it is already stated that the bias should be estimated as integrated white noise in addition to a white noise component directly on the signal to correspond to the measurement noise Using these assumptions together with the INS equations derived in 4 5 the model takes form as BD Ray 4 26 V o b 4 vl 4 27 which can be rewritten as 0 R O 0 0 x 0 0 1I x I0 v 4 28 0 0 0 0 I I 00 I 0 v o ro a 7 4 29 where x p v bf lp The estimated position and speed error are then subtracted from the original states calculated from the IMU measurements As the bias is likely to be following each of the accelerometers it needs to be transformed back to the platform frame immediatetly after calculated This transformation is to be performed using the mean value of the attitude measurements dating back to the previous filter update 44 EQUATIONS SUMMARY 43 4 4 Equations Summary To give a quick summary of the equations given earlier in
16. is adviced to be placed near the center of inertia of the vessel while stationary relative to Earth The center of inertia will shift whenever in motion particularly for maritime vessels v t 2 2 ROTATION MATRICES AND TRANSFORMATIONS 13 First assume the IMU being placed at the center of inertia but with an gles relative to the body frame noted as 6 0 V The rotation to the body frame is then performed by multiplying with 2 12 substituting the respective angles with the ones relative to the body frame Rotation to the tangent plane can be performed by adding the body frame Euler angles to the relative angles Second should the platform be positioned elsewhere the first step is needed to align the axis Assume r is a vector denoting the correct displacement from the center given in body frame coordinates As the IMU will detect accelerations of the given point regardless of position relative to the object it is placed on the position can be calculated as usual and the position displacement subtracted from the position p R p jr 2 13 For speed and accelerations the equation needs to be differentiated First considering speed d v p g op 1 2 14 Riv Rip 2 15 Since the rotation matrix is constant its derivative is equal to zero The same can be done for the acceleration yielding a Roa 2 16 VeRy 2 17 14 CHAPTER 2 REFERENCE FRAMES AND TRANSFORMATIONS Chapter
17. not frequency dependent and more locally varying making it difficult to estimate Exosphere Thermosphere 110 lonosphere 100 Stratosphere Figure 3 2 Illustration of Earth and its atmosphere 6 Selective Availability Selective Availability SA is an artificial feature designed by the U S De partment of Defense Its purpose is to introduce slowly changing random errors to avoid the GPS being used for military purposes by other than the U S or terrorist activites However this feature is currently disabled since 2000 and will therefore not be discussed further Multipath Multipath errors are a result of signals reflected off surfaces on the way to or in the vincinity of the receiver causing the signal to travel further than the 3 2 GLOBAL POSITIONING SYSTEM 27 direct path thus causing it to use a longer time to the receiver or the same signal to arrive twice C A multipath errors are usually from 0 1 3 m but errors up to 100 m have been reported For L1 carrier phase the error is assumed to be less than 5 cm Should a signal arrive either twice after a newer signal due to multipath the old signal will simply be neglected However if the signal simply is delayed and no other signals arrive at the receiver in the mean time it is nearly impossible to separate the error from other errors However some precautions can be made e Using an antenna with low gain at low and negative elevations due to most re
18. the report to be used in the actual integration process This section is reccomended to be read after having obtained an understanding of what has been written in the previous sections of this report and is meant to be used as a reference for implementation Furthermore it will only assess the equations used in the actual implementation of the total system 4 4 1 IMU w sin A vg Avp v R fP g d 2wie sin A un P Zwie cos A vp 2w e cos AJUE AUN 4 30 1 E Eg t me 0 t p V R Ivy 0 Re h cos A 0 V 4 31 0 0 1 which is to be computed at each iteration 4 4 2 GPS For the GPS measurements the speed needs to be derived and transformed to the tangent plane Ra h 0 0 v RE vi O Ra th cos A 0 v s 4 32 0 0 Il 4 4 3 Kalman Filtering Kalman filtering is applied whenever a GPS measurement arrives The fil tering is done with regard to the error equations D Rey 4 33 cb V b r 4 34 44 CHAPTER 4 INTEGRATING GPS AND INS which can be written on state space form 0 R o0 00 x 0 0 I x I O v 4 35 0 0 O 0 I 100 10 y I o JE 4 36 using x pP v bt lis These equations are put on discrete form and fed into the Kalman filter equations given in 4 20 4 25 X k 1Xk 1 Py Aj1Pp 1AL 1 Que 4 38 K P CF CP C Ra 4 39 P 1 KOP x K ly Cix 4 41 Furthermore as the bias is subtract
19. 03 13 D 03 03 I3 03 03 13 C 13 03 03 bias zeros 3 1 Calculating gravity lambda 63 36 gamma_a 9 78049 f 1 298 257223563 m 0 00344978650684 f_2 1 5 2 m 1 2 f72 26 7 f m 15 4 m 2 f_4 1 2 xf 2 5 2 fx m g_const gamma_a 1 f_2 f_4 sind lambda 2 1 4 f_4 sind 2 lambda 2 This is the gravity calculated using the gravity model Flag to denote that imu measurement passes all tests imu isValid 0 prev time Inf fclose instrfind delete instrfind imuopen B 1 MATLAB 71 Defining gain constants for accelerometer and gyro g_acc 9 81 7000 32768000 g_gyro 360 65536 g_rate 8500 32768000 cmd hex2dec 10 00 31 Telling the IMU to write its data Afwrite s imu cmd uint8 Jout fread s imu 7 uint8 while 1 imureader end B 1 7 kalman m function bias P KalmanFilter kalm y P Rt2E dt kalm I eye 9 kalm Q zeros 3 9 zeros 3 3 0 025 eye 3 zeros 3 3 zeros 3 6 0 1 eye 3 eye 3 1E 10 zeros 3 3 zeros 3 3 1 eye 3 kalm A eye 9 kalm A 1 4 Rt2E 1 dt kalm A 2 5 Rt2E 2 dt kalm A 3 6 Rt2E 3 dt kalm A 4 7 dt kalm A 5 8 dt kalm A 6 9 dt kalm_R kalm_C eye 3 zeros 3 6 zeros 3 3 eye 3 zeros 3 3 P pre kalm_A P kalm_A kalm_Q kalm K P pre kalm C inv kalm C P pre kalm C kalm R P kalm I kalm K kalm C P pre x kalm K kalm y bias x B 1 8 p
20. 1 0 75 0 1 0 75 0 1 0 75 Acceleration Accuracy m s 2 ms 0 02 0 02 0 02 Size and operation Bandwith Hz 20 20 100 Price NOK 67 000 407 000 1645 000 Figure 1 1 Statistics of commercial INS GPS solutions Seatex Seapath series As can be seen from Figure 1 1 both the Seapath 100 and 200 is very expensive but has very little deviation The Crossbow NAV420C A 100 is far cheaper almost 5 of the price of the Seapath 200 but in return has a significantly reduced accuracy This system is one of the less expensive products on the marked but still about 3 times the cost of the hardware considered for our purposes Thus a performance close to the NAV420 will be considered satisfactory All the data presented in Figure 1 1 has been obtained from the respective manufacturers by Maritime Robotics Most data is presented as Root Mean Square RMS error with the exceptance of the position accuracy of the Crossbow solution which is given in Circular Error Propable CEP CEP is a common measure of the accuracy of weapons giving the radius of a circle whence the projectile will land 50 of the time Thus the Crossbow will have a position estimate of less than 3 meters 50 of the time Despite all efforts deviation data for solutions when the GPS is disabled has not been obtained This makes it somewhat difficult to come a definite conclution when comparing two different solutions as this is one of the key attributes of usch a
21. 19 ECEF Since the GPS estimates of the position usually are given in the ECEF frame it can be convinient to also present the inertial measurements using the same coordinates One drawback with using this conversion is the complicated gravity calculations The position p and the earth relative velocity v is given as p Rip 3 20 vo RIV 3 21 The relative velocity is given as in the previous example The differential equation is given by applying the theorem of Coriolis ve R v v 3 22 By inserting 3 19 into 3 22 the complete expression is obtained v Ri f g 20v f gt 207 vi 3 23 The specific force in ECEF coordinates is given as F RT 3 24 5e PeOb gt 1 b _ b b ye where R R Q2 is valid using w wa Riu Local Geodetic Local geodetic coordinates are the most trivial coordinates for most people and corresponds with the coordinates most commonly used in daily life The frame is also known as the NED frame since the coordinates are the same as north east and down Furthermore the coordinates are fairly easy to convert from GPS coordinates and are also practical with regard to control purposes The geodetic velocity is related to the inertial velocity through v R v 3 25 and is differentiated using the theorem of Coriolis v RIV v 3 26 20 CHAPTER 3 SYSTEM CONSEPT By inserting 3 19 the result yields v RUM vi f g iev
22. 3 System Consept 3 1 Inertial Measurement Units As opposed to the GPS which relies on external synchronization to achieve an estimate of the position an Inertial Navigation System measures acceleration using the physical laws of nature A pure INS consists only of accelerometers and gyros and is based on the principle that estimates of the position and velocity is obtained by integrating the acceleration When reffering to Inertial Measurement Units it is assumed to consist of a total of three accelerometers and three gyros Proper IMUs are generally very expensive due to need for very accurate measurements The reason why accurate measurents are needed is that the acceleration is integrated twice to obtain the position Any error in the acceleration measurement will also be integrated and cause a bias on the estimated velocity and a continous drift on the position estimate unless corrected Correcting this error is impossible on a pure INS unless recalibrated or reset An INS is commonly aided by magnetometers being able to detect attitude and GPS to measure position For aviation applications hydroaltimeters are common to detect altitude As a single GPS receiver is unable to detect drift in attitude it relies on external aiding to correct the error through e g GPS compass or magnetometer Originally INS was developed for missiles but is today also commonly found in airplanes submarines spacecraft and ships l
23. 4 Acceleration total t total t delta t for i 1 3 subplot 3 1 i plot total t delta t total tl prev imu a i imu a i plot total t delta t total tl prev bias i bias i r plot total t 30 total t O 0 b axis total t 30 total t 5 5 ylim auto end prev bias bias drawnow GPS time GPS time delta t end B 1 5 IMU fm function ret R phi IMU f x u a e ax 1 e72 1 exsin x 1 1 72 71 5 R_lambda omega_ie 7 292115E 5 lambda h v_n v_e v_d x phi out 1 out 2 out 3 out 4 out 5 out 6 ret out 1 1 1 1 1 1 a sqrt 1 exsin x 1 1 72 1 R_phi x 3 1 x 4 1 4 dPhi dt 1 R lambda x 3 1 cos x 1 1 x 5 1 dlambda dt x 6 1 dh dt u 1 1 out 1 1 2 omega ie sin x 2 1 x 5 1 out 2 1 x 3 1 u 2 1 out 1 1 2 omega ie sin x 2 1 x 4 1 out 1 1 2 omega ie cos x 2 1 x 6 1 u 3 1 out 1 1 2 omega ie cos x 2 1 x 5 1 out 2 1 x 1 1 70 APPENDIX B SOURCE CODE B 1 6 sysinit m clear all close all global omega_ie global Q global R lat 10 32 60 20 60 60 long 63 25 60 46 60 60 text fopen imu txt w total_t 0 prev_bias 0 0 0 R diag 5 ones 1 3 ones 1 3 Q eye 9 omega_ie 7292115e 11 I3 eye 3 03 zeros 3 3 A 03 13 03 03 03 I3 03 03 03 4B
24. E CODE sind psi cosd theta cosd psi cosd phi sind psi sind theta sind phi cosd psi sind phi sind psi sind theta cosd phi sind theta cosd theta sind phi cosd theta cosd phi Bibliography 1 2 3 4 5 6 7 8 9 10 11 12 13 14 Volker Bertram Unmanned surface vehicles a survey Technical report ENSIETA 2008 K Betke The NMEA 0183 protocol Technical report 2001 M Brain http www howstuffworks com picture only A K Brown and Y Lu Performance test results of an integrated GPS MEMS inertial navigation package ION GNSS 2004 GlobalSat Technology Corporation Product user manual GPS receiver engine board EM 411 http www globalsat com tw accessed on 10 5 2008 J Doe http www mistupid com picture only C Dubay and T Johns WAAS DGPS and the Mariner s Toolkit Technical report US Coast Guard 2000 M K Eriksen Ground station and hardware peripherals for fixed wing UAV Cyberswan Master s thesis Norwegian University of Science and Technology 2007 J A Farrell and M Barth The Global Positioning System amp Inertial Navi gation R R Donnelley amp Sons Company 1999 R Henriksen Stokastiske systemer 1998 O C Jakobsen Low cost integrated navigation systems Master s thesis Norwegian University of Science and Technology 2004 Microstrain 3DM GX1 gyro enhanced orientation sensor http w
25. EM CONSEPT where 0 r q 0 T 0 p q p 0 i e 05 is the skew symmetric form of wt p q r T This differential equation is more straight forward than when using any other reference frame However it is not very commonly used due to the difficulty in calculating the gravity G R Inertial Position Earth Relative Velocity This representation of variables is not very commonly used as they are im practical to work with However it is useful as a means to derive the differ ential equations of other more common representations and is therefore of importance Here p is defined as the position vector relative to the center of earth and for simplicity the inertial and ECEF orgins are coincident the center of the earth p Rp 3 16 Furthermore the differentiated earth relative position is the same as the earth relative speed p ve By using the theorem of Coriolis this rela tionship can be written as Lpi v Qipi 3 17 which is differentiated into d ue d d ger qv tup dv Gp 91 rp Fv eR Op ov Tp QOL p Div 3 18 In this equation 4 p represents inertial acceleration 22 p is the local centrifugal acceleration while Qi v denotes the Coriolis acceleration The 1 centrifugal acceleration is the same as the gravity resulting in oy f g Qv 3 19 where the calculation of f is shown in 3 13 3 15 3 1 INERTIAL MEASUREMENT UNITS
26. MO two for RS 485 Operating Temperature 40 to 70 C with enclosure 40 to 85 C without enclosure Size Weight 65x90x25 mm with enclosure 42x40x15 mm without enclosure 74 6 g with enclosure 25 8 g without enclosure Shock Limit 1000 G s unpowered 500 G s powered Table A 1 Microstrain 3DM GX1 specifications 12 A 2 GLOBALSAT EM 411 A 2 GlobalSat EM 411 General Chipset SiRF StarIII Frequency L1 1575 42 MHz C A code 1 023 MHz chip rate Channels 20 channel all in view tracking Sensitivity 159 dBm Position 10 m 2D RMS 5 m 2D RMS WAAS enabled Accuracy Velocity 0 1 m s Time lus synchronized to GPS time Datum Default WGS 84 Acquisition Time Reacquisition 0 1 s average Hot start 1 s average Warm start 38 s average Cold start 42 s average Dynamic Conditions Altitude 18 000 m max Main power input Power consumption Velocity 515 m s max Acceleration Less than 4g Jerk 20m sec Power 4 5V 6 5V DC input 60mA Protocol Electrical level Baud rate Output message TTL level Output voltage level OV 2 85V RS 232 level 4 800 bps NMEA 0183 GGA GSA GSV RMC VTG GLL Physical Characteristics Dimension Operating temperature 30mm x30mmx10 5mm 0 3mm 40 to 85 Table A 2 GlobalSat EM 411 specifications 5 62 APPENDIX A DATA SHEETS Ap
27. ONING SYSTEM 29 two different frames depending on the measurement For position ECEF geodetic A h is used while for speed the tangent plane NED has been chosen In order to transform the GPS data the position already given in ECEF geodetic coordinates is differentiated To transform the speed 3 31 is used bJ ma 9 97 fo A 9 umm UE 3 40 h 0 0 i UD UN Roth 0 0 vg 0 Ry h ecos O J 3 41 Up 0 0 1 h where Ra and Ra is given in 3 32 3 33 30 CHAPTER 3 SYSTEM CONSEPT 3 3 Hardware Selection As this report will consider the implementation of an integrated navigation system hardware is needed The IMU has been chosen mainly according to price while the GPS module has been recycled from an earlier Unmanned Aerial Vehicle UAV project at NTNU 8 3 3 1 IMU The IMU chosen is the Microstrain 3DM GX1 This consists of three ac celerometers three gyros and three magnetometers thus giving acceleration in 6 degrees of freedom 6 DOF and position in 3 DOF 3DM GX1 p MicroStrain Figure 3 4 Microstrain 3DM GX1 IMU 12 The 3DN GX1 guarantees an accelerometer bias stability of 0 010 G where G is the Earth s gravitational constant and 0 7 sec for the gyros To correct the gyro error the magnetometers can be used operating at a bias stability of 0 010 gauss The sensors have a bandwidth of 100 Hz and are able to detect accelerations up to 500 G The IMU tran
28. SE ee ev f g Qne 202 v 3 27 where Qni Qne Qie giving the specific force as MENT 3 28 and R R Q are valid using 2 05 RE OL ML 3 29 where c is measured by the gyros w wie cos 0 sin is the rate of inertial rotation of Earth and 7 is the transport rate of the navigation frame relative to earth 3 1 4 Position Dynamics By assuming the velocity vector has been found like in the previous section the position can be found by integrating se Ad 3 30 where p 0 is the initial position To obtain geodetic position from tangent plane velocity coordinates the fol lowing differential equation can be used aT 0 UN A p een O vg 3 31 h 0 0 1 UD where R is the radius of curvature in a meridian at a given latitude and R is the transverse radius of curvature a l e 1 sini d a vA esin B a being the radius at equator and e being the eccentricity Ry 3 32 Ry 3 33 3 1 INERTIAL MEASUREMENT UNITS 21 3 1 5 Gyros The equations obtained in the previous section applies for both strap down and stabilized platform systems The difference between the two lies in the stabilized being actuated to maintain its alignment with a given reference system while the strap down system is as its name suggests rigidly attached to the body of the vessel Mathematically the difference lies in the rotati
29. STEM CONSEPT 3 3 3 NMEA 0183 The NMEA 0183 is a standard developed by the National Marine Electronics Association and uses ASCII communication over the serial line The standard is commonly used in marine measurement devices like GPS receivers The message header can be divided in three parts first a sign implying the start of a message followed by a prefix containing a device identifier specified by the protocol The final three letters contain the type of message being sent however only two are particularly relevant for obtaining the position hence GGA and RMC Only these two will be explained in the following text More information can be found in 2 The GGA suffix denotes the Global Positioning System Fix Data It con tains time position and fix related data for a GPS receiver Table 3 3 shows a GGA message with a description of each message part For messages using the NMEA 0183 standard each part of the message are separated with the delimiter The RMC suffix indicates the contents of the message being the Recom mended Minimum Navigation Information An example message is shown in Table 3 5 Name Example Description Message ID GPGGA Message header Time UTC 123456 123 hhmmss sss Latitude 6325 0840 ddmm mmmm N S Indicator N North N or South S Longitude 01024 1304 dddmm mmmm E W Indicator E East E or West W GPS Quality Indicator 1 See Table 3 4 Number of satellites 09 00 12 HDOP 2 1 Horizon
30. acceleration and speed which can be calculated by differentiating the position measurements As a contrast an Inertial Navigation System INS only measures the forces acting on an Inertial Measurement Unit IMU and can thus be used to cal culate both speed and position estimates without differentiating In addition to achieving higher bandwiths on the measurements than GPS this approach gives the estimates the same bandwith as the acceleration measurements Furthermore the INS does not rely on external signals and is therefore not susceptible to jamming nor the problem of areas lacking satellite coverage Using an INS isn t problem free however as it suffers from problems with drift of speed and position and is also significantly more expensive than GPS equipment IMUs are placed on a platform inside on the vessel referred to as IMU platform This will be discussed later in the report There are several reasons why an integration of GPS and an INS is desirable Generally an INS gives several advantages that the GPS system lacks and vice versa Several sources approaches this problem e g 4 8 9 and 11 The main reasons for performing such an integration is e The INS results are available whenever the GPS measurements are un available due to e g interference or jamming and can also be applied to underwater vehichles e The INS measurements are obtained without significant time delays e The INS provides acceleration
31. ar 1 1 GPS chksm data star 1 1 star 1 2 fprintf text 4s 4s 4s As As As AS Nn GPS lat 1 2 GPS lat 3 numel GPS lat GPS long 1 2 GPS long 3 numel GPS long GPS S0G GPS COG GPS time end elseif GPS name GPGGA commas find data star find data if numel commas 14 GPS time data commas 1 1 commas 2 1 GPS lat data commas 2 1 commas 3 1 GPS NS data commas 3 1 commas 4 1 GPS long data commas 4 1 commas 5 1 GPS EW data commas 5 1 commas 6 1 B 1 MATLAB 65 GPS posFix data commas 6 1 commas 7 1 GPS nSat data commas 7 1 commas 8 1 GPS HDOP data commas 8 1 commas 9 1 GPS h data commas 9 1 commas 10 1 GPS AltUnit data commas 10 1 commas 11 1 GPS GeoSep data commas 11 1 commas 12 1 GPS GeoSepUnit data commas 12 1 commas 13 1 GPS diffAge data commas 13 1 commas 14 1 GPS diffRefId data commas 14 1 star 1 1 GPS chksm data star 1 1 star 1 2 fprintf text As AsASAS AS AS AS Nn GPS 1at 1 2 GPS lat 3 numel GPS 1at GPS long 1 2 GPS long 3 numel GPS long GPS h GPS posFix GPS time end end end B 1 3 imuopen m Ahhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhh name imuopen m Author Haakon Ellingsen Created 13 05 2008 Last modified 16 05 2008 Description Opens a connection to the IMU Opening the serial port the first command is platform computer
32. ated INS GPS while in motion N e Speed N m s ee 20 0 5 10 15 20 25 30 35 40 45 50 55 Time s Speed E m s 3o8 5 10 15 20 25 30 35 40 245 50 55 Time s Speed D m s e 5 10 15 20 25 30 35 40 45 50 55 Time s Figure 5 10 Dynamic speed estimate with GPS dropout It is also worth noting what happens when the GPS is offline during when moving through the course As can be seen from Figures 5 9 and 5 10 the results are somewhat positive despite a high magnitude on the position and a high drift downwards Regardless the deviation north and down are very large compared to what is really acceptable As the IMU is also supposed to be placed on a USV it will also become subject to waves and vibrations To simulate this the IMU was kept nearly stationary while being rotated and moved back and forth in small circles 5 2 DYNAMIC TESTS 53 E Estimated z o o a E Estimated w GPS e o a E Estimated o GPS 8 a Figure 5 11 Position deviation during external influence with GPS Speed N m s e 5 10 15 20 25 30 35 40 45 50 55 Time s Speed E m s a 5 10 15 20 25 30 35 40 45 50 5 Time s Speed D m s e 5 1 15 20 25 30 3 40 do SJ 156 Time s Figure 5 12 Speed deviation during external influence with GPS 54 20 Acc x m s e 20 0 20 Acc y m s 20 0 20 Acc z m s e 20 0 Figure 5 13 Acceleration during external in
33. be assumed to be the common mode error and is subtracted from the measurement The disadvantage of using DGPS is the lack of global coverage Figure 3 3 Graphical presentation of differential GPS 14 The United Stated Federal Aviation Administration FAA and the United States Department of Transportation DoT has constucted a feature called the Wide Area Augmentation System WAAS 7 Described shortly WAAS is similar to the DGPS system but only accessible in North America and is mainly designed for aerial applications WAAS is not certified for maritime navigation and it is claimed that DGPS has a higher accuracy whenever close to the reference station Due to WAAS being unavailable outside North America and the fact that it has not yet been certified for marine applications 7 it will not be discussed further 3 2 3 Velocity Measurements Navigational charts and other tools such as GPS measurements are usually given in an ECEF geodetic reference frame using latitude longitude and altitude denoted as A h respectively For presentation or using as aid to an INS both ECEF and a local geodetic frame can be used The local geodic frame is usually preferred whenever operating within a small area For larger areas of operation ECEF is concidered inertial There are several ways to perform the integration between GPS and INS depending on the frame desired In this report integration is performed in 3 2 GLOBAL POSITI
34. e local gravity vector g will be defined as f or B g p Mi flip 3 6 The INS measures the forces in its current position denoted as f which has the following properties PER 3 7 2The equivalent of S wi or w x 3 Assuming no rotation 3 1 INERTIAL MEASUREMENT UNITS 17 where 1 Ayw Quy Reel UL ds 3 8 Owv Qwu 1 represents the accelerometer displacement where auw is to be read as the posi tive rotation angle about platform w axis from the uw plane to accelerometer u axis 3 1 2 System Equations The system can be put on first order form p v 3 9 y a 3 10 Furthermore the theorem of Coriolis 9 gives R REINE 3 11 3 10 can be rewritten as Mi f g R f g 3 12 where g is the gravity vector and f is the total force acting on the system 3 1 3 Velocity Dynamics Inertial Frame The differential equation for the position vector in an inertial frame is given in 3 4 P f G p 3 13 As previously stated this measurementcan be integrated once to obtain iner tial speed and twice for inertial position The force in a given inertial frame can be written as a rotation from the body frame fs Rf 3 14 where Ri is the rotation matrix from the body frame to the inertial frame This can be used to find the differential equation for the rotation matrix in accordance with the theorem of Coriolis stated in 3 11 VU RIO 3 15 18 CHAPTER 3 SYST
35. e representation with GPS data as measurements y and the inertial measurements as input u in a Kalman filter While this seems straight forward it has three major drawbacks 1 The Kalman filter covariance equations have to be calulated at the rate of the inertial measurements As these equations require much CPU the bandwith of the inertial measurements are highly restricted 2 The measurements have highly deterministic components that will be represented by ad hoc models in the state space representation 3 The states can change dramatically between iterations which require high filter bandwith Complementary filtering considers the inertial measurements and mechaniza tion equations as two separate systems giving the Kalman filter a reference 35 36 CHAPTER 4 INTEGRATING GPS AND INS trajectory When new GPS measurements arrive the INS states are com pared to the GPS data By running the difference between the two sets of data through a second Kalman filter a set of error equations can be esti mated This implementation enables the covariance update equations to be calcu lated only at each GPS measurement reducing the total computational load 4 1 2 Complementary Filtering For the complementary filtering approach there are three different solutions loosely coupled GPS position and range aided INS plus a tightly coupled approach The loosely coupled GPS position aided INS is shown in Figure 4 1 This method us
36. ed from the respective states the esti mated bias needs to be set zero every iteration Chapter 5 Results All simulations and tests have been performed in Matlab using Runge Kutta 4 for solving the differential equations A c implementation has also been made for real time implementation based on the existing code at Maritime Robotics but this code had performance issues due to only using an old laptop with insufficient computation capabilities Also due to this thesis being somewhat delayed all results have been ob tained during the summer vacation meaning that any tests on a boat was hard to perform Thus all tests have been made within a small area while attempting to simulate a USV like environment At the Microstrain IMU the z axis is pointing upwards despite the drawing on the IMU telling it points down meaning that the z axis needs to be inverted The other axis are correct as they are marked on the IMU com pleting the right hand rule Also the yaw angle seems to be measured against west instead of north meaning 90 has to be subtracted from the yaw angle Both the z axis correction and the yaw angle correction are contrary to what stands in the manual and it is unknown whether this is a manufacturing error Covarianse matrices has been found by sampling GPS and IMU measure ments and calculating the covariance offline All test results shown are taken from several samples in an attempt to capture the worst
37. es the position from the GPS to calculate the INS error which is fed back to the INS system This method is very simple and requires little processing of the GPS output 5 x a v INS ptop gt ea Mes ox Vehicle E p NES p gt GPS Kalman filter Figure 4 1 GPS position aided INS 9 A similar method is the GPS range aided INS shown in Figure 4 2 This method uses a range predictor to transform the INS measurements to a range prediction while comparing these to the GPS range measurements This approach gives a better system performance than the position aided system but requires the understanding and processing of GPS data which may be unavailable for off the shelf hardware Finally the tightly coupled solution uses INS data as feedback also to the GPS This decreases the bandwith of the tracking loop and makes the system less prone to interference and jamming However this approach requires not only access to the range data but also to the hardware and Kalman filter design in the GPS Thus this approach is generally not available when using an off the shelf receiver 4 1 APPROACHES 37 a v x 5x x gt INS gt A Y B Range Prediction Vehicle p p p T GPS Pau Kalman filter SX Figure 4 2 GPS range a
38. f 3 equals approximately 5 RMS 97 98 CHAPTER 6 DISCUSSION AND SUMMARY could work to some extent in a car or similar where the external noise on the vehicle is reduced and there exists a set of feasable positions based on road networks in addition to continously obtaining speed measurements from the vehicle The angular measurements are pretty accurate and can be used directly as a compass However there has not been performed any corrections for position in the vincinity of the north pole to correct for the displacement of the magnetic north pole 6 2 Future Work First of all if any further work is to be performed it would be absolutely neccesary to invest in a more expensive IMU since this lacks the performance to yield good results Regardless of hardware there are a few things that has not been implemented due to insufficeint time on the schedule e Quaternions should be used in stead of rotation matrices This will lead to a lower computational load as well as avoiding the singularity present at 90 pitch even though this is as good as impossible on a USV e Temperature tests should be performed to determine the ratio between temperature and bias IMUs are known to be more temperature depen dant than time dependant meaning the change in bias will be larger whenever there is a change in temperature than it will just over time e Obtaining a GPS receiver able to deliver pseudoranges leading to the possibility of des
39. f this thesis is to create an integrated application able to achieve performance of existing solutions three times the cost The implementation has been made in real time in c and off line in Mat lab However the c code has not been sufficiently tested due to computer processing problems Also the code has not been tested on an actual un manned surface vehicle The integrated solution worked sufficently when the GPS was online How ever during GPS droupout the result is subject to high position drift re sulting in position errors of up to 400 meters after 20 seconds Although it is unknown quite how large the position deviation of other existing solutions are However high drift during GPS dropouts renders the IMU estimates quite useless for navigation Thus this experiment has been unsuccessful II ABSTRACT Acknowledgements I would like to thank my advisor Morten Breivik for helping me and advising me on the progress of my work and my report I d also like to thank Vegard Evjen Hovstein and the rest of the Maritime Robotics team for giving me access to their analysis of existing solutions and making this work possible by financing the inertial measurement unit A special thanks to Arild Hepso for his assistance with Maritime Robotics existing c code and incorporating my code into theirs Finally Pd like to thank Bj rnar Vik for giving me valuable insight in equa tions and integration techniques as well as sharing fro
40. flecting surfaces being below the receiver e If possible place the receiver higher than the highest reflector e Do not accept signals from satellites at low elevation as these travel nearly parallell to the surface and are thus more error prone to reflec tions Avoiding multipath is extremely important when choosing sites for DGPS reference stations as multipath errors are non common mode errors and will manifest themselves in the error estimate sent from the station Receiver Noise The receiver noise is assumed to be white and is a result of error in measuring transit time The error is due to factors such as nonlinearity and thermal noise It is highly dependent on hardware selection in the receiver and will therefore vary with the quality of the GPS receiver 3 2 2 Differential GPS As seen from Table 3 2 the most significant contribution to the error on the estimated position comes from the common mode errors By setting up a stationary reciever at a known position the readings will be affected by the same common mode errors as any other reciever in the area Since the position is already known the error can be calculated by subtracting the measurement from the receiver by the known position This error can be broadcasted together with the corresponding clock readings Thus every 28 CHAPTER 3 SYSTEM CONSEPT receiver in the vicinity can obtain the error and adjust for it The error calculated by the reference station will
41. fluence with GPS 25 30 Time s 25 30 Time s 25 30 Time s CHAPTER 5 RESULTS 35 40 35 40 35 40 45 45 45 As seen from Figure 5 11 and 5 12 the estimate is able to follow the GPS relatively good but struggles more than in the previous experiments How ever when GPS dropout is introduced as seen in Figure 5 14 and 5 15 the estimate drifts off fast The RMS values for position and speed can be seen in Table 5 3 North East Down Velocity RMS 1 3686 1 5217 1 3715 Position RMS 2 8322 2 8322 0 5399 Table 5 3 RMS values integrated INS GPS with external disturbances 5 2 DYNAMIC TESTS 95 Pos N m Estimated GPS D Time s Estimated GPS O Time s 400 Estimated E 200 o 0 d 200 5 10 15 20 25 30 35 40 d 5 Time s Figure 5 14 Position deviation during external influence with GPS dropout 50 Speed N m s e 5 10 15 20 25 30 35 40 45 50 55 Time s w 20 w 10 D a ce 10 5 10 15 20 25 30 35 40 45 50 5 Time s 50 Speed D m s o 5 a 15 20 25 30 3 AO do 350 156 Time s Figure 5 15 Velocity deviation during external influence with GPS dropout 56 CHAPTER 5 RESULTS Chapter 6 Discussion and Summary 6 1 Discussion The low cost application proved to work more or less satisfactory whenever GPS coverage were availiable In static conditions it were
42. hksm out 22 out 23 Calculating checksum calc_chksm dec2hex imu header imu rollt imu pitch imu yawtimu a_x timu a_ytimu a_z imu omega_x imu omega_y imu omega_z imu timer n_byt size calc_chksm Setting valid flag if prev_time Inf imu_isValid 0 prev_time imu timer display Can t integrate without previous time Run again else B 1 MATLAB 67 imu isValid 1 end else Resetting valid flag imu isValid 0 display C Size received is wrong end if imu isValid if imu chksm calc chksm n byt 2 3 n byt 2 Means that the checksums matches scaling the measurements imu roll imu roll g gyro imu pitch imu pitch g gyro imu yaw imu yaw g gyro if imu a_x gt 32768 Overflow meaning negative number imu a_x imu a x 65536 g acc else imu a x imu a x g acc end if imu a_y gt 32768 Overflow meaning negative number imu a_y imu a y 65536 g acc else imu a y imu a y g acc end if imu a_z gt 32768 Overflow meaning negative number correcting imu a_z imu a z 65536 g acc else imu a z imu a z g acc end if imu omega x gt 32768 Overflow meaning negative number imu omega x imu omega x 65536 g rate else imu omega x imu omega x g rate end if imu omega_y gt 32768 Overflow meaning negative number imu omega y imu omega y 65536 g rate else imu omega_y imu omega y g rate end if imu omega_z gt 32768 Overflow meaning negative
43. i 2 8 cos cos A cos sin A sin 12 CHAPTER 2 REFERENCE FRAMES AND TRANSFORMATIONS 2 2 3 Body to Tangent Plane Transformation This transformation is performed by using the Euler angles derived from the body frame and transforming via one axis at atime By chosing to start with the rotation around the z axis the new coordinates x y 2 id is obtained a cos w sinlyw 0 y sin v cos v 0 y 2 9 z 0 0 1 z The same method is applied to the two remaining axes g cos 0 O sin 0 E Wl 0 1 0 y 2 10 a sin 0 O cos 0 EA u 1 0 0 g v 0 cos Q sin Q y 2 11 0 sin d cos Q 2 and thus having obtained the body frame coordinates These can be com bined by multiplication yielding cos W sin v O cos 0 O sin 0 1 0 0 v sin b cos w 0 0 1 0 0 cos Q sin Q 0 0 1 sin 0 O cos 0 0 sin d cos Q cos p cos 0 sin b cos 0 sin 0 sin y cos 0 cos w sin sin _cos wW cos d sin w sin sin d cos O sin v sin w sin d cos w sin cos d cos W sin b sin v sin 0 cos o cos 0 cos b RN 2 12 2 2 4 Platform to Body Transformation Assume a rigidly attached point on the vessel where the measurement sensors are placed The center of this platform is usually chosen as the origin of the platform frame As the placement of this platform will differ from each vessel no standard transformation can be performed For inertial sensors the platform
44. ided INS 9 a v x dx x INS gt A v x x Range Prediction Vehicle p p p j L GPS Ee Kalman fitter Figure 4 3 Tightly coupled GPS aided INS 9 38 CHAPTER 4 INTEGRATING GPS AND INS 4 2 GPS Position Aided INS The GPS position aided INS is defined as an INS using the GPS position to adjust for the drifting due to INS acceleration error 9 It is useful to put the system equations on state space form x Ax Bu f x u Dv 4 1 y Cx w 4 2 where x is the states u is the input f x u is the non linear parts of the differential equation v is the process noise and w is the measurement noise Combining 3 27 with 4 1 usingx p v the system can be written as a IOI 0 0 AA PA where b is the accelerometer bias calculated in the Kalman filter and 20 cos A Wey 2Wi 4 4 6 2w sin A For simplicity I and O is used throughout this document meaning I3 3 and 03x3 unless stated otherwise The input u is the measured force from the INS f f f For a simpler transition between GPS and INS states 4 3 can be rewritten to give its position in ECEF geodetic coordinates 9 A h This is easilly achieved using R given in 3 31 yielding sai i wen 02 onte 9 y I 0 x w 4 6 Combining these equations the resulting equations yields o Wie sin A ug vp Y f bf g 6 2wie sin Ajuy 2w e cos A up 4 7
45. igning a tightly coupled integrated solution e Using another state of the art system in order to benchmark the system properly Appendix A Data Sheets The data sheets listed in this Appendix is taken from the specifications listed at the manufacturers websites and reproduced here in this report A 1 Microstrain 3DM GX1 99 60 APPENDIX A DATA SHEETS Orientation Range 360 full scale FS all axes Matrix Quaternion modes Sensor Range Gyros 300 sec FS Accelerometers 5 G s FS Magnetometers 1 2 gauss FS A D Resolution 16 bits Nonlinearity Accelerometer 0 296 Gyro 0 296 Magnetometer 0 496 Bias Stability Accelerometer 0 010 G s Gyro 0 7 sec Magnetometer 0 010 gauss Orientation Resolution lt 0 1 minimum Repeatability 0 20 Accuracy 0 5 typical for static test conditions 2 typical for dynamic test conditions and for arbitrary orientation angles Output Modes Digital Outputs Analog Output Option Digital Output Rates Serial Data Rate Matrix Quaternion Euler angles and 9 scaled sensors with temperature Serial RS 232 RS 485 optional 0 5 volts FS for Euler angles pitch 90 roll 180 yaw 180 100 Hz for Euler Matrix Quaternion 350 Hz for nine orthogonal sensors only 19 2 38 4 115 2 Kbaud programmable Supply Voltage Supply Current Connectors 5 2 VDC min 12 VDC max 65 milliamps One keyed LE
46. in groups common mode and non common mode errors 9 Common mode errors are errors that occur within a limited geographic region and are equal for all recievers within the region As opposed to common mode non common mode errors refer to reciever individual errors that can occur independent of location Receiver Clock Bias Table 3 2 shows a list of errors divided into common mode and non common mode One error that is not included in this list is the receiver clock bias This error is equal for all signals and can therefore be estimated if signals from four satellites are available by 5 Approximately 3 10 m s 3 2 GLOBAL POSITIONING SYSTEM 25 Errors Standard Deviation Common Mode Selective Availability 24 0 Ionosphere 7 0 Clock and ephemeris 63 6 Troposphere 0 7 Noncommon Mode Reciever Noise 0 1 0 7 Multipath 0 1 3 0 Table 3 2 GPS Error Sources reproduced from 9 1 Differentiating two simoultaneous measurements from the same receiver 2 Calculating an estimate at each time step 3 Estimating the error as a state using a dynamic model and Kalman filtering Although not in Table 3 2 this error is receiver individual and can be viewed as non common mode Atmospheric Effects One problem with calculating the distance from the satellite based on the time a signal uses on its journey from the satellite to the receiver is that the signal has different speed based on the substance it travels through
47. ion during external influence with GPS 53 Speed deviation during external influence with GPS 53 Acceleration during external influence with GPS 54 Position deviation during external influence with GPS dropout 55 Velocity deviation during external influence with GPS dropout 55 IX LIST OF FIGURES List of Tables 3 1 3 2 3 3 3 4 3 5 5 1 5 2 9 3 Al A 2 WGS 84 ellipsoid properties 15 22 GPS Error Sources reproduced from 9 25 The NMEA GGA message 2 aa sce sanert ae 32 GPS Quality Indicator Values 2 tores 22 Gn 33 The NMEA RMC message 2 33 RMS values integrated INS GPS in static conditions 48 RMS values integrated INS GPS while in motion 52 RMS values integrated INS GPS with external disturbances 54 Microstrain 3DM GX1 specifications 12 60 GlobalSat EM 411 specifications 5 61 XI XII LIST OF TABLES Notation and Abbreviations Notation Meu v x ms a Wa Standard typing means that x is a scalar Boldfaced typing means that R is a matrix or vector True value of x Estimated value of x Measured value of x Denotes the error x X R is the rotational matrix that transforms a set of vectors from frame a to frame b dx The dot represents time differentialization of x x 7 Rate of angular rotation of frame a with respect to frame b coordintized i
48. ly known path were set up in order to test the performance After approximately 40 seconds the IMU is assumed to be proper calibrated and is moved in a square like path of about 5x5 meters always moving in the direction of IMU s x axes During the first run the GPS is constantly online The results is shown in Figures 5 4 5 6 As can be seen from Figure 5 4 the position deviation is not that large The measurements from the GPS are a bit to low according to the real path whereas the true position lies somewhere in between the two Regardless the Kalman filter corrects the INS estimates quite conciderably which can easily be seen from the speed estimates in Figure 5 5 The estimated bias is shown in Figure 5 7 It may also be worth taking a look at Figure 5 8 showing the yaw angle measurement which is quite accurate 5 2 DYNAMIC TESTS 49 Estimated 10 GPS Pos N m 0 5 10 15 20 25 30 35 40 45 50 5 Time s E ul o Estimated P mo GPS 0 5 10 15 20 25 30 35 40 45 50 5 Time s 5 Estimated E GRE a 8 a 0 5 10 15 20 25 30 35 40 45 50 5 Time s Figure 5 4 Dynamic postion estimate with GPS coverage 10 Speed N m s e 10 0 5 10 15 20 25 30 35 40 45 50 55 Time s a 20 w 10 D a ce 10 5 10 15 2 25 30 35 40 45 50 5 Time s w5 E ag Ed o eo a e 5 0 5 a 15 20 25 30 3 AO 45 SJ 156 Time s Figure 5 5 Dynamic speed estimate with GPS coverage
49. m his previous hands on experience IH IV ACKNOWLEDGEMENTS Contents Abstract I Acknowledgements HI Notation and Abbreviations XIII 1 Introduction 1 TI Motivation sanieren IA a ehr 1 L2 Backo round up Det yom dhe Ps OR qoe qe dos 1 1 2 1 Survey of Existing Solutions qo EE ERG 3 13 Outlines co Exo eel pu dex EAN SE See Be ae ag 5 2 Reference Frames and Transformations T 2 1 Reference Frames i aos a eon d E ke T 2 1 1 Inertial Fame ono soeur a SE GE T 2 1 2 Earth Centered Earth Fixed T 2 1 3 Local Geodetic or Tangent Plane 9 214 Body BEAR eodd pp ere qu 9 2 2 Rotation Matrices and Transformations 2 2 2 222 20 10 2 2 4 ECEF Geodetic to ECEF Rectangular 10 2 2 2 ECEF to Tangent Plane Transformation 11 2 2 3 Body to Tangent Plane Transformation 12 2 2 4 Platform to Body Transformation 12 3 System Consept 15 3 1 Inertial Measurement Units 0 4 15 3 1 1 Accelerometers zw bude oh sr Ta GREC V 16 3 1 2 System Equations uox sede rete REGES 17 3 1 3 Velocity Dynamics a ae 17 3 1 4 Position Dynamics ul Bees a 20 3 13 TGS see ed was be ae asked eee 28 21 VI CONTENTS 3 1 6 Gravity Model xx pesca e erm 21 3 2 Global Positioning System 23 3 2 1 HATO SOUECES TALA qe ddr eo dioe RS 24 3 2 2 Differential GPS SAT Le aa 27 3 2 3 Velocity Measurements a ran 28
50. n Unmanned Surface Vehicles USV has been in used in service of the military since World War II but has not become largely popular until the 1990s It is still commonly found in military applications but is also increasingly found in research vessels USVs are commonly used to search for underwater mines or underwater activ ities investigate the sea bottom rescue vessels reconnaissance and surveil lance vessels or as a support vehicle for e g an autonomous underwater vehicles Unmanned surface vehicles are usually relatively small often the size of a recreational watercraft below 15 meters and so far a USV exceeding 100 tons has yet to be found 1 As USVs are usually quite small they are also somewhat inexpensive compared to larger vessels Furthermore as they are autonomous or remotely operated proper navigation systems are neccesary to be able to implement successful control algorithms As proper navigation systems usually also has a high price they are concidered to be unfit for these applications as they will drastically increase the price of the USV 1 2 Background The Global Positioning System GPS represents an inexpensive and global method of obtaining the position of a vessel Although the measurements 1 2 CHAPTER 1 INTRODUCTION are highly subjected to noise the accuracy can be improved by applying the principle of differential GPS However the system gives a low bandwith especially when it comes to
51. n frame a Variables D Lo a3 8 e 2 Owe oo DE X Euler angles Rate of angluar rotation Rate of angular acceleration Position vector Velocity vector Acceleration vector Skew symmetric form of w wx cross product Longitudinal horizontal speed surge Lateral horizontal speed sway Vertical speed heave Body frame roll rate Body frame pitch rate Body frame yaw rate Roll in Euler angle Pitch in Euler angle Yaw in Euler angle Latitude longitude XIII XIV NOTATION AND ABBREVIATIONS Abbreviations CET Circular Error Propable DGPS Differential GPS DoD United States Department of Defense DOF Degrees Of Freedom DoT United States Department of Transportation ECEF Earth Centered Earth Fixed FAA United States Federal Aviation Administration GPS A Global Positioning System IMU nertial Measurement Unit INS Inertial Navigation System HDOP Horizontal Dilution Of Precision L1 fi GPS carrier frequency L2 fo GPS carrier frequency NED North East Down frame NMEA National Marine Electronics Association NTNU Norwegian University of Science and Technology RMS Root Mean Square SA Selective Availability USV Unmanned Surface Vehicle UTC Universal Time Coordinated VDOP Vertical Dilution Of Precision WAAS Wide Area Augmentation System WGS World Geodetic System Chapter 1 Introduction 1 1 Motivatio
52. n integrated solution An approximate relationship between performance and price is shown in Fig ure 1 2 beginning at 40 000 NOK It is unsure whether this will be accurate 4 CHAPTER 1 INTRODUCTION T e co Position Deviation m RMS 100 200 300 400 500 600 700 800 Price 1000 NOK Figure 1 2 Accuracy versus price for commercial INS GPS solutions for prices below 60 000 since RMS position deviation for differential GPS range as low as 3 meters 1 3 OUTLINE 5 1 3 Outline Chapter 1 discusses the motivation for the assignment and shows data for existing integrated solutions Chapter 2 presents different reference frames and different equations used in order to transform a vector from one frame to another Chapter 3 looks at the history behind the Global Positioning System and inertial navigation It also discusses potential errors they are suspectible to and gives equations for estimating position and speed based on acceleration measurements Chapter 4 outlines the system equations used for integrating the two navi gational systems and the approach to estimate attitude velocity and accel eration Chapter 5 shows the results of the integration and compares them with prop erties from existing solutions Chapter 6 concludes the report and also contains a discussion regarding the concidered product and future work CHAPTER 1 INTRODUCTION Chapter 2 Reference Frames and Transformations 2 1 Reference
53. nd height P A h and is given in the spherical coordinates The system takes its basis in the ECEF rectangular frame The latitude is found by rotating around the z axis until the x axis crosses the projection from the position on to the x y plane The longitude is then found by rotating around the y axis until the x axis coincides with the vector from the center of the Earth to the position The height is the distance from the nearest point normal on the assumed altitude The altitude is assumed to be at the surface of the WGS 84 ellipsoid WGS 84 or the World Geodetic System is an estimate of Earth dating back to 1984 This ellipsiod will be concidered furter upon developing a gravity model in Section 3 1 6 Both ECEF frames are depicted in Figure 2 1 as well as the ENU frame which will be presented later 2 1 REFERENCE FRAMES 9 Geographic Frame The geographic frame is dependent on its origin and is only locally correct It is earth fixed and has its origin at the ellipsoid used to describe the surface of the Earth The x axis points north the y axis towards east and the z axis points down normal onto the ellipsoid Geosentric Frame This frame is equal to the geographic frame with the difference that its z axis is pointing towards the center of the Earth 2 1 3 Local Geodetic or Tangent Plane The local geodetic frame is the frame most people consider when orienting themselves It takes basis of making a fictional tangent
54. number imu omega z imu omega z 65536 g rate else imu omega z imu omega z g rate end 68 APPENDIX B SOURCE CODE imu isValid 1 else Means that the two checksums are different rejecting the data imu isValid 0 display Checksum calculation failed end end if imu isValid Evaluating the timer delta t imu timer prev time if delta t 0 delta t delta t 65536 end prev time imu timer The rollover is accounted for scaling the measurement delta t delta t 0 0065536 fprintf text i1 5 1 15 1 5 1 5 1 i n imu a_x imu a y imu a Zz imu roll imu pitch imu yaw delta t end Updating the rotation matrix Rb2n updateR imu roll imu pitch imu yaw h Solving the differential equation prev_imu imu 4 imu a Rb2n imu a x imu a y imu a z bias 0 0 g_const boat v 2 2 omega ie sind boat long hboat v 1 2 omega_ie sind boat long h boat v 3 2 omega_ie cosd boat long h boat v 2 2 omega_ie cosd boat long imu v imu v imu a delta t imu p imu p imu v delta t figure 1 plot prev imu p 2 imu p 2 prev imu p 1 imu p 1 drawnow 4 figure 2 4 Euler angles h 4 for 1 133 if i Ah hold off hh plot3 0 Rb2n i 1 0 Rb2n i 2 0 Rb2n i 3 hh hold on 4 grid on hh end MATLAB 69 plot3 0 Rb2n i 1 O Rb2n i 2 0 Rb2n i 3 axis 1 1 1 1 1 11 B 1 h h hh h end h figure 3
55. o TA ze set ne 78 B 1 11 updateR m leg ely doe apuesta he 79 Bibliography VIII CONTENTS List of Figures Ll 1 2 2l 3 1 3 2 3 3 3 4 3 5 4 1 4 2 4 3 5 1 5 2 9 3 9 4 9 9 9 6 9 7 9 8 9 9 5 10 5 11 5 12 5 13 5 14 5 15 Statistics of commercial INS GPS solutions 3 Accuracy versus price for commercial INS GPS solutions 4 The Earth with both ECEF frames and the local geodetic frame 8 Concept art of the Global Positioning System 3 24 Illustration of Earth and its atmosphere 6 26 Graphical presentation of differential GPS 14 28 Microstrain 3DM GX1 IMU 12 as ak a dies 30 GlobalSat EM 411 GPS receiver with the RS 232 interface 31 GPS position aided INS 9 36 GPS range aided INS DL 2 20 rare 37 Tightly coupled GPS aided INS 9 3T Stationary position wih GPS dropout 46 Stationary speed deviation with GPS dropout 47 Stationary acceleration deviation with GPS dropout 47 Dynamic postion estimate with GPS coverage 49 Dynamic speed estimate with GPS coverage 49 Dynamic acceleration estimate with GPS coverage 50 Bias estimate during dynamic conditions 50 Measured yaw angle during dynamic conditions 51 Dynamic postion estimate with GPS dropout 51 Dynamic speed estimate with GPS dropout 52 Position deviat
56. obal Positioning System GPS people usually refer to the NAVSTAR GPS which was originally developed by the United States Department of Defense for use in military applications 13 In 1983 a Korean airliner was shot down by the Soviet air force Since the disaster could have been avoided with access to a proper navigation system Ronald Reagan decided to open the NAVSTAR project for the public GPS has since then become very popular due to its low cost availability and accuracy Russia also has a similar global positioning system called the GLONASS and the European Union s Galileo is to be completed around 2011 In addition China s regional satellite system Beidou has been proposed extended to a global system called COMPASS Some GPS receivers make use of both GLONASS and NAVSTAR data to increase accuracy GPS can be divided into space control and user segments 9 e The space segment is the satellites orbiting the earth It consists of six planes with four satellites on each plane for a total of 24 satellites These planes are arranged in such a way that gives at least six satellites line of sight to almost any given point at all times e The control segment monitors the health and status of the satellites It consists of six monitoring stations located in Cape Canaveral Ascen sion Island Kwajalein Diego Garcia Hawaii and Colorado Springs The four first also have each its ground antenna These stations send clock correction
57. on matrix R The strap down system is the easiest to implement and is also the one being imple mented in this report and is therefore considered Regardless both systems requires information of the rotational vector Why Wip Win 3 34 The gyros experience Wip and w RPw For tangent plane navigation Wie cos cos Phi ms Win zx o Wie 0 ate HIR 3 35 A wje sin sin C is valid For obtaining the attitude db 1 sin Q tan 0 cos o tan 0 p 0 costo E q 3 36 Y 0 cos 0 cos 0 T can be used 3 1 6 Gravity Model For use in a geographic reference system the gravity is calculated based on the WGS 84 ellipsoid model 15 The vector is defined as A 0 6 g 0 des 3 37 h g 4With regard to hardware 22 CHAPTER 3 SYSTEM CONSEPT Constant Value a 6378137 0 m f 298287228803 e 8 1819190842622 107 Ya 9 78049000 m s Ww 7292115 0 10 rad s m 0 00344978650684 h f 2m if 20 fm Sm fa rn Table 3 1 WGS 84 ellipsoid properties 15 with 23 Te 2 Y B Ya h fo fa sin 1 4sin 20 3 38 y h y a h Hf m 3f m sin 8 h dep 3 39 The parameter values for the gravity model are given in Table 3 1 For maritime applications 3 39 is not considered since the height is close to constant at h 0 3 2 GLOBAL POSITIONING SYSTEM 23 3 2 Global Positioning System When talking about the Gl
58. or matrix A and the nonlinear errors tend to be neglected to ensure proper observability Furthermore acceptable results are achieved using an error model of only slowly varying bias and white noise The bias differential equation should be modelled as p v 4 13 where v is white noise It is also worth noting that since the IMU chosen in this experiment includes a magnetometer leading to the unit internally calculating position and angular velocity Thus the above equations regarding angular motion has not been used as the IMU angular output was used directly 4 2 1 GPS Since the IMU position is already given in the ECEF geodetic frame no trans formation is required However the velocity vector needs to be transformed into the tangent plane This is an easy process starting with differentiating two position measurements obtaining the ECEF geodetic speed By multi plying this with the matrix gained in 3 31 tangent plane speed is achieved For the GPS its equations are already given in 3 41 UN Roth 0 0 VE 0 Ry h cos 0 RS 4 14 UD 0 0 1 h Thus the esimation error can be calculated as x XGPS XIMU 4 15 4 3 Kalman Filter The Kalman filter is an optimal linear state estimator able to estimate the full system state depending on incomplete and noisy measurement series The theory of the filter dates back to 1960 when Rudolf Kalman proposed the filter to NASA for the Apollo Program The filter come
59. pendix B Source Code B 1 Matlab B 1 1 gpsopen m name imuopen m Author Haakon Ellingsen Created 10 03 2008 Last modified 21 05 2008 Description Opens the port for the GPS Opening the serial port the first command is platform computer specific For a stationary COM port on the windows platform COM1 is usually correct If in doubt check the device manager For linux systems try e g A dev ttyCOM1 or dev ttyUSBO fclose instrfind S gps serial COM13 baudrate 4800 fopen s gps B 1 2 gpsreader m i 1 text fopen gps txt w gpsTime clock save gpsTime 63 64 APPENDIX B SOURCE CODE while 1 data n fscanf s_gps clear GPS if max size data gt 6 GPS name data 2 6 else GPS name 0 end Testing the data received if max find data 1 GPS name 0 end Only interested in the GPS data if GPRMC or GPGGA if GPS name GPRMC commas find data star find data if numel commas 11 GPS time data commas 1 1 commas 2 1 GPS status data commas 2 1 commas 3 1 GPS lat data commas 3 1 commas 4 1 GPS NS data commas 4 1 commas 5 1 GPS long data commas 5 1 commas 6 1 GPS EW data commas 6 1 commas 7 1 GPS S0G data commas 7 1 commas 8 1 GPS COG data commas 8 1 commas 9 1 GPS date data commas 9 1 commas 10 1 GPS magVar data commas 10 1 st
60. plane at the origin just like presenting the globe as a map The x axis points north the y axis towards east and the z axis points down normal onto the ellipsoid there fore also widely known as the NED frame north east down This frame coincides with the geographic frame for a stationary target The difference between the two is that in the latter frame the origin is a projection of the platform origin onto the Earth s geoid Another version of this frame is the east north up frame ENU 2 1 4 Body Frame In the body frame the origin is usually in the center of gravity of the body of the object in question Its x axis points towards the defined front of the object the z axis points down and the y axis points right to complete the right hand rule This frame is together with the NED frame widely used for control purposes The frame represents the vessel states in 6 degrees of freedom 6 DOF known as surge sway and heave u v w and roll pitch and yaw 6 6 y Surge sway and heave is the speed in x y and z respectively and roll pitch and yaw is the vehicle s angular displacement from the NED frame 10 CHAPTER 2 REFERENCE FRAMES AND TRANSFORMATIONS 2 2 Rotation Matrices and Transformations In order to transform states from one frame to another rotation matrices can be used For a rotation matrix subscripted letters indicate the frame it is being transformed from while superscripted letters denote the frame the sta
61. ps data long count O gps speed tmpoff 0 0 0 x if id gps Using ideal gps test purposes only gps speed gps_speed 0 x tilde gps data lat 1 gps data long 1 0 gps_speed x end est s count gps speed gps data time count if count2 gt 10 P eye 9 count2 0 B 1 MATLAB 75 end bias P kalman x tilde P Rt2E totalTime X x bias 1 6 if numit lt 50 Rb2t updateR totatt 1 numit totatt 2 numit totatt 3 numit else x 1 3 gps_data lat count gps_data long count 0 end biasAcc biasAcc inv Rb2t bias 7 9 biascount count biasAcc gpspos count gps_data lat count gps_data lat 1 Rt2E 1 gps data long count gps data long 1 Rt2E 2 0 count count 1 totatt 0 0 0 numit 0 x 3 0 end else break end Zend accel2 i a x i a y i a z i biasAcc inv Rb2t 0 O 9 9506 R phi WGS_a 1 WGS_e72 1 WGS_e sin x 1 1 72 71 5 R_lambda WGS a sqrt 1 WGS e sin x 1 1 2 data i totalTime R_phitx 3 1 x 1 gps_data lat 1 R_lambdatx 3 1 cos x 1 1 x 2 gps_data long 1 x 3 x 4 6 acce1 end figure 1 subplot 3 1 1 hold on plot data 1 data 2 plot gps_data time 1 numel gpspos 3 gps_data time 1 gpspos 1 r legend Estimated GPS plot 0 70 0 b axis 0 55 1 1 axis auto y subplot 3 1 2 hold on plot data
62. roc m clear all 72 APPENDIX B SOURCE CODE close all count 1 load imu txt load gps txt id_gps false posoffs 0 10 0 0 numit 0 totatt 0 0 0 count 1 count2 0 roll imu 4 pitch imu 5 yaw imu 6 90 a x imu 1 a y imu 2 a z imu 3 gps_data lat gps 1 gps 2 60 pi 180 gps_data long gps 3 gps 4 60 pi 180 gps data S0G gps 5 gps data COG gps 6 gps_data time gps 7 gps_data time 1 39 gps_data time 1 39 40 startTime gps_data time 1 for i 1 numel mat 1 mat i mat i diag 1 2 0 4 rand 1 3 end 4 a x mat 1 a x 1 numel mat 1 a y mat 2 a y 1 numel mat 1 4 a z mat 3 a z 1 numel mat 1 dt imu 7 WGS_a 6378137 WGS_e 0 08181979099211 x gps_data lat 1 gps_data long 1 O O 0 0 prevAccel 0 0 0 totalTime 0 biasAcc 0 2 0 5 0 hbiasAcc 0 0 017 B 1 MATLAB 73 P eye 9 alltime 0 prevtime 0 prevX x figure 1 subplot 3 1 1 hold on xlabel Time s ylabelC Pos N m subplot 3 1 2 hold on legend Phi lambda h xlabel Time s ylabel Pos E m subplot 3 1 3 hold on legend Phi lambda h xlabel Time s ylabel Pos D m figure 2 hold on figure 3 hold on legend a_x a_y a z
63. s and orbital model to the satellites via the antennas e The user segment is the GPS reciever In order to work it needs an antenna tuned in to the frequencies used by the satellites The reciever also has a high accuracy clock usually being driven by a quarts oscillator The GPS transmits data signal over two main carrier frequencies called L1 and L2 transmitting on 1575 42 and 1227 60 MHz respectively All data is trasmitted using the Coarse Acquisition C A code which is available to the public and the Precise P code used by the military L1 transmits both the C A and the P code while L2 transmits the P code Both frequencies are available for all users but due to encryption of the P code only the C A is usable by the public 24 CHAPTER 3 SYSTEM CONSEPT Figure 3 1 Concept art of the Global Positioning System 3 3 2 1 Error Sources As the GPS signals travel with the speed of light an error of To ms will lead to a measurement error of 300 m Thus the timing needs to be highly accurate in order to give an acceptable measurement The time bias can be assumed equal for all satellites and accounted for by using the measurement from three satellites simultaneously solving four equations with four unknown variables In other words a minimum of four satellites are needed in order to achieve position However several factors still contribute to the GPS error It is appropriate to separate the error sources into two ma
64. s given as MIE 1 ER an 2 2 ROTATION MATRICES AND TRANSFORMATIONS 11 which is used for the transformation between the two frames To calculate rectangular coordinates x N h cos cos A 2 2 y N h cos sin A 2 3 z N 1 e h sin 2 4 The transformation the other way around is a bit trickier and is not very relevant for this report It will thus not be discussed here 2 2 2 ECEF to Tangent Plane Transformation The transformation from ECEF to tangent plane coordinates starts by sub tracting the tangent plane origin given in the ECEF frame from the ECEF coordinates x Gu zo Yo 20 2 5 leaving the two planes with the same origin The next step is performing a rotation around the ECEF z axis until the y axis is aligned with tangent plane east cos A sin A 0 Ri sin A cos A 0 2 6 0 0 1 where A is the longitude By performing a new rotation this time around the aligned y axis until the new z axis is aligned with the tangent plane down cos 3 0 sin 3 Ro 0 1 0 sin 5 0 cos 5 sin O cos 0 1 0 l 2 7 cos 0 sin where q is the latitude Please note that this notation is opposite than that of 9 as the notation used in this report is believed to be more commonly used By combining the two the complete rotation matrix is obtained sin cos A sin sin A cos Ri sin A cos A 0
65. s in many different forms but the one most relevant for this work is the discrete Kalman Filter which also will be the one most thoroughly investigated 4 3 KALMAN FILTER 41 4 3 1 Discrete Kalman Filter The dicrete filter takes its basis in a system written on state space form Xk 1 ArxXk T Bruk Dico 4 16 Yr CpXp vi 4 17 with Ri E vivi 4 19 as covariant matrices for the process and measurement noise respectively It is assumed that both Q and R are known As there are no input u in the system this can be neglected Should the system be observable the Kalman filter can be used To ensure observability the observability matrix C CA O l 4 20 ATIC needs to have rank n n being the number of states Should observability be obtained the fiter process can begin starting with estimation of the state variables X p 1Xk 15 4 21 where xy is the previously calculated state estimate As the bias calculated in the filter is subtracted from the INS states this previously calculated estimate needs to be reset every interation The process is repeated for the covariance matrix P Ap aP A 1 Qu 4 22 With the estimated values in place the Kalman gain and the covariance update can be calculated K PLCI C P C Rg 4 23 P 1 K C P 4 24 42 CHAPTER 4 INTEGRATING GPS AND INS using the previously calculated estimates The Kalman gain are used to calculate the final
66. smits data over an RS 232 serial line depending on the com mand sent to the device As a standard the 3DN GX1 transmits a message each time the given command byte is sent but continuous mode can be enabled making the IMU transmit a given message continuously 3 8 HARDWARE SELECTION 3l 3 3 2 GPS Receiver The GPS module as mentioned earlier has been recycled from a UAV project detailed in 8 The GPS consists of a GlobalSat EM 411 receiver mounted on a RS 232 interface as shown in Figure 3 5 Figure 3 5 GlobalSat EM 411 GPS receiver with the RS 232 interface The module has no internal power and thus needs external powering to function It operates at 4 5 6 5 V but the RS 232 board contains a voltage converter making it able to operate at 9 V meaning it can be powered by an external 9 V power supply or a PP3 battery most commonly used in smoke detectors This receiver has a position accuracy of 10 meters or 5 meters with Wide Area Augmentation System WAAS enabled The EM 411 module does support DGPS but its manual does not list the accuracy for DGPS However the accuracy can be assumed close to that of WAAS The EM 411 receiver supportes NMEA 0183 protocol which is also the stan dard setting discussed in Section 3 3 3 The price of the EM 411 is less than 500 NOK More data on the module is provided in Appendix A For hardware interested readers the full EM 411 manual is available at 5 32 CHAPTER 3 SY
67. specific For a stationary COM port on the windows platform COM1 is usually correct If in doubt check the device manager For linux systems try e g A dev ttyCOM1 or dev ttyUSBO S imu serial COM13 baudrate 115200 DataBits 8 fopen s imu 4 Creating and sending enter continous mode command to the IMU g g meaning it will send data all the time Is this wise in matlab due to bad reading methods cmd hex2dec 107 7007 7317 write s_imu cmd uint8 66 APPENDIX B SOURCE CODE B 1 4 imureader m name imureader m Author Haakon Ellingsen Created 13 05 2008 Last modified Description Reads data from the IMU and processes the data received making it hcmd hex2dec 10 00 317 cmd hex2dec 31 Telling the IMU to write its data fwrite s imu cmd uint8 Reading the IMU data out dec2hex fread s imu 23 uint8 Processing the data if size out 23 2 imu header hex2dec out 1 imu roll hex2dec out 2 out 3 imu pitch hex2dec out 4 out 5 imu yaw hex2dec out 6 out 7 imu a x hex2dec out 8 out 9 imu a_y hex2dec out 10 out 11 imu a z hex2dec out 12 out 13 imu omega x hex2dec out 14 out 15 imu omega y hex2dec out 16 out 17 imu omega z hex2dec out 18 out 19 imu timer hex2dec out 20 out 21 imu c
68. tal dilution of precision Altitude 56 1 Altitude relative to geoid Units M Units of antenna altitude Geoidal separation 10 1 WGS 84 height deviation Units M Units of geoidal separation Age of diff GPS data Null field without DGPS Diff ref station ID 0000 0000 1023 Checksum 6B Table 3 3 The NMEA GGA message 2 3 8 HARDWARE SELECTION 33 Value Description 0 Fix not available or invalid 1 GPS SPS Mode fix valid 2 Differential GPS SPS mode fix valid 3 GPS PPS Mode fix valid Table 3 4 GPS Quality Indicator Values 2 Name Example Description Message ID GPRMC Message header Time UTC 123456 123 hhmmss sss Status A A data valid V data invalid Latitude 6325 0840 ddmm mmmnm N S Indicator N North N or South S Longitude 01024 1304 dddmm mmmm E W Indicator E East E or West W SOG 0 16 Speed over ground knots COG 46 98 North relative to north deg Date 220508 ddmmyy Magnetic variation E East or West deg Checksum 6B Table 3 5 The NMEA RMC message 2 34 CHAPTER 3 SYSTEM CONSEPT Chapter 4 Integrating GPS and INS 4 1 Approaches There are several ways to implement a GPS INS integration First of all we differ between the direct filtering and the complementary filtering approach 9 4 1 1 Direct Filtering Direct filtering is perhaps the most intuitive of the two It uses position and velocity as states in a state spac
69. tes are being transformed to Thus R p means the states are transformed from the platform to the body frame 2 2 1 ECEF Geodetic to ECEF Rectangular The geodetic coordinates given in latitude and longitude P A only gives coordinates in two directions namely north and east The height is then assumed zero unless stated otherwise according to the ellipsoid model of the Earth s geoid being used In particular the WGS 84 ellipsoid is commonly used As already stated the model parameters will be considered further when discussing the gravity model in Section 3 1 6 but for now only a few parameters is of importance The ellipsoid has two constants needed to define the model These are the semimajor and semiminor axis noted as a and b respectively The semimajor axis is the longest of the two going horizontally from the center of the Earth along the xy plane in Figure 2 1 The semiminor axis is thus the one pointing vertically along the z axis The length of the semimajor axis is a 6378137 0 while the semiminor axis is only needed to calculate the flatness of the ellip soid defined as AED ee il 398257228563 Since 15 already has provided f an explicit value of b is not needed Fur thermore the eccentricity of the ellipsoid is defined as e f 2 f 8 1819190842622 1072 Finally the length of the normal of the ellipsoid from the surface of the ellipsoid to the intersection with the ECEF z axis i
70. th cosd lambda O 00 1 B 1 10 sysinit m clear all close all global omega_ie global Q global R lat 10 32 60 20 60 60 long 63 25 60 46 60 60 text fopen imu txt w total_t 0 prev_bias 0 0 0 R diag 5 ones 1 3 ones 1 3 Q eye 9 omega_ie 7292115e 11 I3 eye 3 03 zeros 3 3 A 03 I3 03 03 03 13 B 1 MATLAB 79 03 03 03 4B 03 I3 D 03 03 I3 03 03 I3 C 13 03 03 bias zeros 3 1 Calculating gravity lambda 63 36 gamma a 9 78049 f 1 298 257223563 m 0 00344978650684 f_2 f 5 2xm 1 2 f 72 26 T f mt15 4 m 2 f_4 1 2 f 2 5 2 fx m g_const gamma_a 1 f_2 f_4 sind lambda 2 1 4 f_4 sind 2 lambda 2 This is the gravity calculated using the gravity model Flag to denote that imu measurement passes all tests imu isValid 0 prev_time Inf fclose instrfind delete instrfind imuopen Defining gain constants for accelerometer and gyro g acc 9 81 7000 32768000 g gyro 360 65536 g rate 8500 32768000 hcmd hex2dec 10 00 31 1 Telling the IMU to write its data Afwrite s imu cmd uint8 Jout fread s imu 7 uint8 while 1 imureader end B 1 11 updateR m function Rb2t updateR phi theta psi Rb2t cosd psi cosd theta sind psi cosd phi cosd psi sind theta sind phi sind psi sind phi cosd psi sind theta cosd phi 80 APPENDIX B SOURC
71. ww microstrain com accessed on 10 5 2008 C Pellerin United states updates global positioning system technology Tech nical report www america gov 2006 accessed on 8 3 2008 G Pendleton http www directionsmag com picture only 2002 81 82 BIBLIOGRAPHY 15 The World Geodetic System Its definition and relationships with local geode tic systems Technical report The United States Department Of Defense 1984

fulltext - DiVA Portal

Contents

Download Pdf Manuals

Related Search

Related Contents