Tesis de posicionamiento
Upcoming SlideShare
Loading in...5
×

Like this? Share it with your network

Share
  • Full Name Full Name Comment goes here.
    Are you sure you want to
    Your message goes here
    Be the first to comment
    Be the first to like this
No Downloads

Views

Total Views
935
On Slideshare
935
From Embeds
0
Number of Embeds
0

Actions

Shares
Downloads
6
Comments
0
Likes
0

Embeds 0

No embeds

Report content

Flagged as inappropriate Flag as inappropriate
Flag as inappropriate

Select your reason for flagging this presentation as inappropriate.

Cancel
    No notes for slide

Transcript

  • 1. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation Anders Engman July 6, 2006
  • 2. AbstractThis report looks into four different filters for sensor data fusion of three sensors,GPS, gyro and odometry. The complementary characteristics of the sensors meanthat they together give an increased navigational performance. The report mainlyfocuses on the filters ability to estimate the filter states that is being used to supportthe navigation filters during GPS blockouts. The filters ability to estimate a path isnot the main focus.The sensor data used in the report comes from a wheel based experiment platformequipped with a GPS, a gyro and odometry. The encoders for the odometry is con-structed and evaluated in the report. The GPS is used as a measurement of position,heading and speed, while the gyro and odometry is either used as a measurement,or to drive the system equations, in different combinations. The four filters exam-ined estimates the vehicle position, heading, wheel radii, rear wheel axis width,gyro bias, gyro scale factor, wheel radii error and heading angular speed in differ-ent combinations. SammanfattningDenna rapport undersöker fyra stycken filter för sammanvägning av sensordatafrån tre olika sensorer, GPS, gyro och odometri. Sensorernas komplementära egen-skaper innebär att de tillsammans ger ökad navigeringsprestanda. Rapporten un-dersöker i första hand de olika filtrens förmåga att estimera de tillståndsvariablersom används för att stötta navigeringen vid bortfall av GPSdata. Filtrets förmågaatt estimera en färdad bana kommer först i andra hand.Sensordata som används i rapporten kommer från en hjulbaserad experimentplat-tform försedd med en GPS, ett gyro och odometri. Encodrarna för odometrin kon-strueras och utvärderas i rapporten. GPSen används som mätning för position,riktning och hastighet medan gyro och odometri antingen som mätning, eller attdriva systemekvationerna, i olika kombinationer. De fyra filtren som undersöks es-timerar, förutom fordonets position och riktning, även hjulradier, hjulbasavstånd,gyrobias, gyroskalfaktor, hjulradiefel och riktningens vinkelhastighet i olika kom-binationer.
  • 3. LIST OF FIGURES2.1 Different types of gyros. . . . . . . . . . . . . . . . . . . . . . . 42.2 Encoder pulse waves. . . . . . . . . . . . . . . . . . . . . . . . . 62.3 Parameter definitions. . . . . . . . . . . . . . . . . . . . . . . . . 73.1 The vehicle shown without the top cover. . . . . . . . . . . . . . 123.2 Block diagram of the autonomous vehicle system. . . . . . . . . . 133.3 Reflective tachometer schematics. . . . . . . . . . . . . . . . . . 143.4 Opacity encoders. . . . . . . . . . . . . . . . . . . . . . . . . . . 153.5 Encoder wheel assembly. . . . . . . . . . . . . . . . . . . . . . . 173.6 Encoder wheel assembly. . . . . . . . . . . . . . . . . . . . . . . 184.1 Parameter definitions. . . . . . . . . . . . . . . . . . . . . . . . . 225.1 The GPS data derived from the test run in Kista. . . . . . . . . . . 395.2 The estimated vehicle path generated by odometer data. . . . . . . 395.3 The gyro path derived from the test run. . . . . . . . . . . . . . . 405.4 Wheel radius estimate plot with the nominal value removed. . . . 415.5 This plot shows the innovation of the position of the Kalman filter together with their 1-sigma standard deviation. . . . . . . . . . . . 415.6 This plot shows the innovation of heading of the the Kalman filter together with their 1-sigma standard deviation. . . . . . . . . . . . 425.7 This plot shows the innovation of speed of the the Kalman filter together with their 1-sigma standard deviation. . . . . . . . . . . . 425.8 This is the estimated path when the GPS has been active during the whole path. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 435.9 Figure of the filter estimated path when GPS data is unavailable during the last lap. . . . . . . . . . . . . . . . . . . . . . . . . . . 435.10 This plot shows the integrated distance error that the filter is doing. 445.11 Heading error plot. . . . . . . . . . . . . . . . . . . . . . . . . . 445.12 Wheel radius estimate plot with its one and three sigma standard deviation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 455.13 Base width estimate plot with one and three sigma standard deviation. 455.14 Position innovation plot with one and three sigma standard deviation. 46 —page i—
  • 4. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation 5.15 Heading innovation plot with one and three sigma standard deviation. 46 5.16 Velocity innovation plot with one and three sigma standard deviation. 47 5.17 The plot shows filtered position estimation where the GPS has been available during the whole estimation time. . . . . . . . . . . . . 47 5.18 Estimated path where the GPS was turned off during the last lap. . 48 5.19 Position difference between filter where GPS is used for the whole path and where it is turned off during the last lap. . . . . . . . . . 48 5.20 Heading estimation difference between the path using GPS for the whole path and the path where the GPS was turned off during the last lap. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49 5.21 Estimation of the gyro bias. . . . . . . . . . . . . . . . . . . . . . 50 5.22 Scale factor estimation. . . . . . . . . . . . . . . . . . . . . . . . 50 5.23 The Kalman filter position innovation. . . . . . . . . . . . . . . . 51 5.24 The Kalman filter heading innovation. . . . . . . . . . . . . . . . 51 5.25 Filter estimated path with GPS data during the whole test time. . . 52 5.26 Filter estimated path with GPS data removed during the last lap. . 52 5.27 Position drift plot. . . . . . . . . . . . . . . . . . . . . . . . . . . 53 5.28 Heading difference between estimated path where GPS has been used during the whole path and where it has been disregarded dur- ing the last lap. . . . . . . . . . . . . . . . . . . . . . . . . . . . 53 5.29 Wheel radius estimate plot with its one and three sigma standard deviation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54 5.30 The bias estimate. . . . . . . . . . . . . . . . . . . . . . . . . . . 54 5.31 Position innovation plot with one and three sigma standard deviation. 55 5.32 Heading innovation plot with one and three sigma standard deviation. 55 5.33 Speed innovation plot with one and three sigma standard deviation. 56 5.34 Path estimated using GPS during the whole test time. . . . . . . . 56 5.35 Path estimated with the GPS disregarded during the last lap of the path. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57 5.36 The position difference between the filter run with GPS during the whole path and the filter disregarding the GPS during the last lap. 57 5.37 The heading difference between the filter run with GPS during the whole path and the filter disregarding the GPS during the last lap. 58 —page ii—
  • 5. LIST OF TABLES3.1 Karnaugh map for software comparison minimization. . . . . . . 195.1 The advantages and disadvantages of the different filters. . . . . . 58 —page iii—
  • 6. CONTENTS1 Introduction 1 1.1 Background . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1 1.2 Motivation for Positioning . . . . . . . . . . . . . . . . . . . . . 2 1.3 Problem Definition . . . . . . . . . . . . . . . . . . . . . . . . . 22 Sensors and Sensor Fusion 3 2.1 Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3 2.1.1 Gyro . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3 2.1.2 GPS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4 2.1.3 Odometry . . . . . . . . . . . . . . . . . . . . . . . . . . 5 2.2 Sensor Fusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6 2.2.1 GPS-Gyro Integration . . . . . . . . . . . . . . . . . . . 7 2.2.2 GPS-Odometry Integration . . . . . . . . . . . . . . . . . 7 2.2.3 GPS-Gyro-Odometry Integration . . . . . . . . . . . . . . 8 2.3 Kalman Filtering . . . . . . . . . . . . . . . . . . . . . . . . . . 8 2.3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . 8 2.3.2 The System Model . . . . . . . . . . . . . . . . . . . . . 8 2.3.3 Observer . . . . . . . . . . . . . . . . . . . . . . . . . . 8 2.3.3.1 Kalman Gain K and Covariance P . . . . . . . 9 2.3.3.2 Kalman Filter Dynamics . . . . . . . . . . . . . 9 2.3.3.3 Filter Initialization . . . . . . . . . . . . . . . . 9 2.3.4 Difference Between Sensors . . . . . . . . . . . . . . . . 10 2.3.5 Linear and Nonlinear Functions and Measurements . . . . 10 2.3.5.1 Linear and Nonlinear Convergence . . . . . . . 10 2.3.6 Extended Kalman Filter Mathematics . . . . . . . . . . . 103 Hardware Implementation 12 3.1 The Autonomous Vehicle . . . . . . . . . . . . . . . . . . . . . . 12 3.2 Odometer Construction . . . . . . . . . . . . . . . . . . . . . . . 13 3.2.1 Different Encoders . . . . . . . . . . . . . . . . . . . . . 14 3.2.1.1 Optical Tachometer . . . . . . . . . . . . . . . 14 3.2.1.2 Magnetic Tachometer . . . . . . . . . . . . . . 15 3.2.1.3 From Tachometer to Encoder . . . . . . . . . . 15 —page iv—
  • 7. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation 3.3 The Encoder Solution . . . . . . . . . . . . . . . . . . . . . . . . 16 3.3.1 Encoder Mechanics . . . . . . . . . . . . . . . . . . . . . 16 3.3.2 Encoder Electronics . . . . . . . . . . . . . . . . . . . . 17 3.3.3 Micro Controller Software . . . . . . . . . . . . . . . . . 19 3.3.4 Sensor Error Analysis . . . . . . . . . . . . . . . . . . . 204 Kalman Filter Implementation 22 4.1 Filter and System Variable Definitions. . . . . . . . . . . . . . . . 24 4.2 Odometry-GPS Estimating Wheel Radii. . . . . . . . . . . . . . . 26 4.2.1 Observability . . . . . . . . . . . . . . . . . . . . . . . . 27 4.3 Odometry-GPS Estimating Wheel Radii and Rear Wheel Axis Width 27 4.3.1 Observability . . . . . . . . . . . . . . . . . . . . . . . . 29 4.4 Odometry-Gyro-GPS estimating Gyro Bias and Scale Factor . . . 29 4.4.1 Observability . . . . . . . . . . . . . . . . . . . . . . . . 31 4.5 Odometry-Gyro-GPS Estimating Wheel Radiuses Error and Gyro Bias . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31 4.5.1 Observability . . . . . . . . . . . . . . . . . . . . . . . . 33 4.6 System and Measurement Noises Q and R . . . . . . . . . . . . . 34 4.6.1 System Noise . . . . . . . . . . . . . . . . . . . . . . . . 34 4.6.1.1 Wheel Radius Noise . . . . . . . . . . . . . . . 34 4.6.1.2 Rear Wheel Axis Width Noise . . . . . . . . . 35 4.6.1.3 Odometer Position Noise . . . . . . . . . . . . 35 4.6.1.4 Odometer Angle Noise . . . . . . . . . . . . . 35 4.6.1.5 Heading Angular Velocity . . . . . . . . . . . . 35 4.6.1.6 Rate Gyro Noise . . . . . . . . . . . . . . . . . 36 4.6.2 Measurement Noise . . . . . . . . . . . . . . . . . . . . 36 4.6.2.1 GPS Position Noise . . . . . . . . . . . . . . . 36 4.6.2.2 GPS Velocity Noise . . . . . . . . . . . . . . . 36 4.6.2.3 GPS Heading Noise . . . . . . . . . . . . . . . 365 Kalman Filter Performance 38 5.1 Odometry-GPS Estimating Wheel Radiuses . . . . . . . . . . . . 40 5.1.1 Filter Conclusions . . . . . . . . . . . . . . . . . . . . . 44 5.2 Odometry-GPS Estimating Wheel Radiuses and Rear Wheel Dis- tance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45 5.2.1 Filter Conclusions . . . . . . . . . . . . . . . . . . . . . 49 5.3 Odometry-Gyro-GPS Estimating Gyro Bias and Scalefactor . . . 49 5.3.1 Filter Conclusions . . . . . . . . . . . . . . . . . . . . . 51 5.4 Odometry-Gyro-GPS Estimating Wheel Radiuses Error and Gyro Bias . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52 5.4.1 Filter Conclusions . . . . . . . . . . . . . . . . . . . . . 56 5.5 Filter Comparison . . . . . . . . . . . . . . . . . . . . . . . . . . 566 Conclusions and Future Work 59 6.1 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59 6.2 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60 6.2.1 Reference Path . . . . . . . . . . . . . . . . . . . . . . . 60 6.2.2 Error Detection Algorithm . . . . . . . . . . . . . . . . . 60 —page v—
  • 8. 6.2.3 Longer Data Set . . . . . . . . . . . . . . . . . . . . . . 60 6.2.4 C Code Implementation . . . . . . . . . . . . . . . . . . 60 6.2.5 Mathematically Driven System . . . . . . . . . . . . . . . 60 6.2.6 Stopping Estimation . . . . . . . . . . . . . . . . . . . . 60 6.2.7 More Advanced Filter . . . . . . . . . . . . . . . . . . . 61Bibliography 62 —page vi—
  • 9. CHAPTER 1 IntroductionThis report is written as an master thesis for the Royal Institute of Technology(KTH) in Stockholm, Sweden on commission of the Swedish Defence ResearchAgency. I wish to pay a special thanks to Peter Strömbäck and Bengt Boberg forthe endless help with the project, my tutor at FOI, Petter Ögren for helping me startup at FOI, my tutor at KTH, Magnus Lindé for the extra help when I needed it, myexaminer Karl Henrik Johansson for the help with the thesis.1.1 BackgroundBy direct controlling a vehicle over a radio link, either by looking out from thevehicle using a camera or looking directly at the vehicle, the person holding theremote control device is navigating through the terrain. The person controlling thevehicle visually identifies the position of the platform and decides what to do next.Autonomous vehicles need to make the same kinds of decisions.One way to do this is to give the autonomous vehicle a camera connected to acomputer. The computer then identifies objects on the camera image and navigatesusing these. A computer is not nearly as advanced as a human brain and positionusing images is very computationally demanding in all but the most basic cases.An easier way for the autonomous vehicle to solve this problem of navigation is togive it a map of places where it can and can not move. For the autonomous vehicleto be able to use this map its position on the map, more or less exact, needs to beknown. There are a number of different devices developed that can keep track ofposition and heading of vehicles. In this thesis focus lies on using gyro, odom-etry and GPS. An INS keeps track of all the forces applied onto the vehicle andby integrating those forces the position, speed and orientation can be calculatedfrom initial values. Using the GPS the instantaneous position of the vehicle canbe decided, and this position has a limited absolute error. Using an odometer thenumber of revolutions the wheels turn is counted and thus the distance the vehiclehas traveled can be calculated. The calculations needed to keep track of the vehicleare a lot lower using these sensors than for the camera imaging navigation. —page 1—
  • 10. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation1.2 Motivation for PositioningThe need for the autonomous vehicle to know its position very exactly has morethan one reason. One is to keep itself on the passable parts of the map or just tofollow a number of preset waypoints. An autonomous vehicle most often has atleast one more mission then just to navigate. One might be to identify the positionof objects. For the autonomous vehicle to find a position of an object with any kindof precision it first needs to know its own position and heading with high precision,since it can only identify the position of the object in comparison to itself. Addinga low certainty of the position of the vehicle itself to the low certainty of the sensoridentifying the objectt’s position relative to the vehicle will give a poor total cer-tainty of the object position.1.3 Problem DefinitionThe objective of this thesis is to derive and implement sensor fusion of a GPS re-ceiver, an automotive grade gyro and an odometer to obtain a cheap, recyclablenavigation system. These three sensors have three unique sets of properties andusing sensor fusion the complementary properties can be used to obtain reliablenavigation.The sensors are placed on a wheeled platform and an onboard computer performsdata logging of the sensor data. The sensor fusion is to be implemented and testedin matlab using the logged data. The vehicle is outfitted with a PC-104 computerfor all onboard computations, a camera, an gyro, a GPS receiver and W-LAN. Theodometer solution is to be developed during the project.The first task is to derive a solution for the odometer, to produce the mechanicsfor the odometer and to make a mathematical model for it. Then a GPS-odometry,GPS-gyro and GPS-odometry-gyro fusion filter should be developed and evaluated. —page 2—
  • 11. CHAPTER 2 Sensors and Sensor Fusion2.1 SensorsThere are a number of sensors that can be used for navigation. All of them havetheir advantages and drawbacks. For this thesis GPS, odometry and a gyro werechosen to be evaluated. Gyro because it cannot be jammed and has a high reso-lution and high sample rate, GPS because it gives a global position with limitedabsolute error, and odometry because it cannot be jammed, does not drift while thevehicle is standing still, and has high resolution and sample rate.2.1.1 GyroThe gyroscope is a flywheel turning at high velocity hanging on three near frictionless axis so that the flywheel does not change rotational axis even if the casingholding the gyroscope does. The angular difference between the flywheel and thecasing can then be measured. A good gyroscope is an advanced and expensivepiece of equipment. Recently, solid state gyroscopes using micro technology havebeen developed. These are smaller and a lot cheaper. They are not really gy-roscopes since they do not have a flywheel, but since they solve the exact sameproblem, they may be called gyroscopes.The gyro, also called rate gyro, however is not perfect. The rate gyro in this thesis isa "MicroElectroMechanical Systems gyro" (MEMS gyro). It has two main errors.One is a scale factor error, meaning that the output from the rate gyro has a scalefactor multiplied with it, thus making the output either smaller or larger dependingon the scale factor being larger or smaller than one. The other error is a bias,meaning that the signal from the gyro at standstill is not zero but rather showseither positive or negative rotation. The measured gyro signal can thus be writtenas r(t) = sr(t) + b + v(t), ˜ (2.1) —page 3—
  • 12. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation (a): Gyroscope (b): MEMS Gyro Figure 2.1: Different types of gyros.where r(t) is the signal from the rate gyro, s is the scale factor, r(t) is the real ˜rotational velocity and b is the bias and v(t) is white measurement noise.One can estimate the angular change from initial direction to current direction ofa rate gyro by integrating the signal. To do this the error parameters have to betaken care of in one way or another, or the estimated direction will drift away fastfrom the real one. A rate gyro such as the one used in this thesis can only measureangular change along one axis.2.1.2 GPSThe Global Positioning System is used to get a global position with limited error.It is based on a number of satellites which send coded signals to a receiver. Thereceiver internally generates signals identical to the ones from the satellites and bycorrelating these with the incoming signals a measurement of signal propagationtime is obtained. This time corresponds to different distances of different satellitesand the position of the receiver can be calculated using the known position of thesatellites.The time (range) measurements are afflicted with several errors where receiverclock error, satellite clock error, ionosphere and troposphere delays, satellite posi-tion error and noise are some of the most prominent ones. These errors all affectthe errors on calculated position [1]. In this thesis the position obtained from thereceiver is used with a simplified error model where the position error is regardedas white noise, according to v1 (t) ˜ x = x+ , (2.2) v2 (t) —page 4—
  • 13. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation ˜where x is the position indicated from the GPS, x is the actual position and, v1 (t)and v2 (t) are white noise.The GPS is based on very weak signals (10−16 W at the receiver) [1] p.283. Thatis 19 dB lower than the thermal noise floor and therefore the GPS is sensitive tojamming and interference, intentional or unintentional. The receiver also requiresa clear line of sight to the satellites. This is especially difficult to obtain in ur-ban environments and terrain covered with heavy foliage. Another solution to thisproblem is collaborative navigation [2]. The position update rate of a modern GPSreceiver is typically a few Hz which can provide problems for a control system ifthe vehicle dynamics are too fast.2.1.3 OdometryOdometry works by having encoders on one or more of the wheels of the platform.Encoders measure the rotational velocity of an axis, here the wheel axis. An en-coder consists of two tachometers. These are placed with a small shift betweenthem so that when one of them has its transition from either low to high or high tolow, the other is just in between two transitions. In this way the square waves fromthe tachometers have a phase shift, the first tachometer waveform being ahead ifthe wheel is turning forwards and the second tachometer waveform being ahead ifthe wheel is turning backwards as shown in figure 2.2. The wheel speeds measuredon a vehicle with two encoders, together with the diameter of the wheel give thedistance the vehicle has moved according to equation 2.3. ωr Rr + ωl Rl v= , (2.3) 2where Rr and Rl are the right and left wheel radius, respectively, ωr and ωl are theright and left wheel speeds, respectively, and v is the rear wheel axis center velocityas defined in figure 2.3.Since the outer wheels travel further than the inner ones, in a turn the direction ofthe vehicle, using two encoders, can also be calculated, thus enabling the calcu-lation of current position relative to the initial position. The equation for vehicleheading using odometry is ωr Rr − ωl Rl r= , (2.4) Bwhere r is the rear wheel axis center angular velocity and B is the rear wheel axiswidth as defined in figure 2.3.A wheel compresses somewhat during travel over uneven terrain, thus changingthe wheel radius, and the wheels might easily slip when moving on gravel at highvelocities or if the platform hits some object in its environment. The encoders alsohave a discretization error. These errors are accumulative over distance and theestimated position drifts significantly after a while of travel. —page 5—
  • 14. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation Figure 2.2: Encoder pulse waves.Detecting wheel slips from the odometry is very hard since there is no way tellingwhen or where they happen. Because of this, the error of the odometry sensor ishard to describe. In this thesis the odometry error is simplified and seen as a whitenoise on the wheel encoders. The measurement in equation 2.5 is used for vehiclevelocity and 2.6 for vehicle angular rate. ˜ ˜ ωr Rr + ωl Rl va (t)Rr + vb (t)Rl v= + , (2.5) 2 2 ˜ ˜ ωr Rr − ωl Rl vc (t)Rr + vd (t)Rl r= + , (2.6) B Bwhere va (t), vb (t), vc (t) and vd (t) is white noise with a large enough variance todescribe both the possible slip error and the quantization error. The positive signbetween the two terms in the noise part of the heading equation 2.6 is due to thatwhite noise is symmetric around zero.2.2 Sensor FusionThe rate gyro and odometer drift over time and distance, the GPS does not. Ifthe autonomous vehiclet’s only mission is to navigate, using only the GPS may insome applications be sufficient. The path planning system within the vehicle justhas to navigate with a margin large enough to the edges of the navigational areathat the vehicle is guaranteed to be within that area.There are, however, a number of reasons why a higher accuracy is needed. Thepositioning of an autonomous vehicle is most often used for more than just gettingfrom point A to point B. For many real life tasks it has a mission other than just —page 6—
  • 15. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation Figure 2.3: Parameter definitions.navigation. One of them might be to find the position of an object by looking at itwith an onboard camera or a laser scanner from a distance. The camera and laserscanner themselves have uncertainties. If one adds the uncertainty of the GPS tothe uncertainty of the platform sensors, the position of the object will not be exactenough since the position and heading information from the GPS is very rough.The vehicle heading is especially important when working with angles to triangu-late an objects position. The position of where the measurement was taken has tohave a small uncertainty. Since the heading information from the GPS has high un-certainty, the angle to the studied position has a high uncertainty as well. The GPSsystem is also dependent on radio connections to the satellites. These connectionscan be jammed either intentional or unintentional. The GPS alone will not be goodenough; an integration of sensors is needed.2.2.1 GPS-Gyro IntegrationGPS has a limited absolute error but the high frequency error is larger, the gyrohas a small high frequency error but a much larger low frequency error causing theposition to drift over time. Combining the two would lead to an more accurate androbust navigation system. In this thesis this is accomplished by using an ExtendedKalman Filter (EKF).2.2.2 GPS-Odometry IntegrationSimilar to the GPS-gyro integration in 2.2.1 filtering together GPS and odometryhas many times proven to give good results [3], [4]. Here, as with the GPS-gyrointegration, the GPS is not totally reliable, and odometry does drift during GPSblackouts. —page 7—
  • 16. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation2.2.3 GPS-Gyro-Odometry IntegrationThe rate gyro and odometry produce a drift when used alone. However, the rategyro and odometry drift is not exactly the same. During a GPS blackout the rategyro and odometry can be weighed together and thus decrease the system drift. Thegyro can then compensate the odometry when the wheels slip during hard turns orwhen running over bumps in the terrain.2.3 Kalman Filtering2.3.1 IntroductionThe Kalman filter (KF) is named after its inventor Rudolf E. Kalman. The filterwas initially developed by Swerling in 1958, and then enhanced by Kalman in1960 and Kalman and Bucy in 1961. A wide variety of Kalman filters have beendeveloped from Kalman’s original formulation. In its original form the Kalmanfilter works for discreet linear systems. Later the so called extended Kalman filter(EKF) was developed which also handles nonlinear systems. In this thesis an EKFis used since the both the system and some measurement functions are nonlinear.A Kalman filter is a special kind of observer that estimates the states of a linearsystem. The issue is that the states cannot be measured directly but rather have tobe estimated though a series of measurements affected with white noise. There area number of filters doing this but the way the variation of the white noise of themeasurements is used is special for Kalman filters.2.3.2 The System ModelThe state of the system to be estimated by the EKF is described by a vector of realnumbers x, each number describing one of the system states. At each discrete timestep k, an nonlinear function f(xk ) is applied to the state to generate the new state.This operation is affected with the system noise, called Qk . Another linear operatoror nonlinear function h(xk ) is then used to transfer the system measurements to thesystem vector. These measurements are affected with measurement noise calledRk . Some of the system states x may be directly observable from the measure-ments, others might be more complicated and might need certain conditions to beobservable at all.2.3.3 ObserverThe observer estimates the states of the system using the model for time update.During the Kalman prediction state the time transfer function is used to propagatethe system state xk to the next time step xk+1 . This can be done using a mathemati-cal model derived as the dynamics of the system or from measurements made thatdescribes the state update. This stage can also be made multiple times between two —page 8—
  • 17. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigationmeasurements.Measurements made by other sensors is then made to correct errors the predictionstage has. These measurements should have other error dynamics than the mea-surements used to drive the prediction stage. Using same measurements in predic-tion state and the measurement state does not deliver any more information to thesystem since the measurement noise will be correlated with the system noise. Theweighting done between the predicted state and the measured state is calculatedusing the noise of the measurement and the covariance matrix, P. The covariancematrix is calculated for every prediction stage as shown in equation 2.7 and de-scribes the system states estimate uncertainties. This weighting between the twonoises is done in an optimal way yielding a minimum variance estimate using thetwo noise matrices P and Q.2.3.3.1 Kalman Gain K and Covariance PP and K is calculated optimaly using equation 2.7 and equation 2.8. How theseequations has been derived is described in [5] p. 117-121. P− = Fk Pk FT + Qk k+1 k (2.7) Kk = P− HT (Hk P− HT + Rk )−1 k k k k (2.8)Where Hk is the linearization of the measurement matrix h(x).2.3.3.2 Kalman Filter DynamicsFor the optimality of K and P to be calculated the noise parameters of R and Q alsohas to be chosen correctly. Q describes as mentioned above the noise of the systemstate prediction. If the system states propagation is driven by measurements thenoise of Q is taken from the error dynamics of the sensors. If the system statespropagation is driven by a mathematical model, the noise of error dynamics of themodel describes Q. Measurement noise R is taken from the sensors error dynam-ics. How the exact correlation between sensor noise and R and Q is can be studiedin [5]. It is usually hard to find an exact value of the noises in R and Q and it iscommon practice to use the calculated R and Q as starting values and then manu-ally tune the matrices until the EKF exhibits the desired performance. If there arenon modeled errors dynamics in the measurements or system model, the noise in Rand Q can be increased so that these errors also include the non modeled errors. Ifthese errors does not have white noise characteristics the filter might not converge,however this is still often used.2.3.3.3 Filter InitializationFor the filter to converge it has to be correctly initialized. The initial covariancehas to be chosen large enough to include the difference between the actual state —page 9—
  • 18. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigationand the estimated state vector.2.3.4 Difference Between SensorsIf the time propagation function is chosen to be driven by sensors the filter will havetwo types of Sensors. Sensors used to drive the system prediction and sensors usedas measurements to correct the prediction. Typically the sensors used to drive thesystem prediction are sensors that have a high output rate and only low frequencynoise. Sensors used for measurements typically have a lower output frequency, ahigher frequency error and preferably no zero frequency error at all.2.3.5 Linear and Nonlinear Functions and MeasurementsIn a linear system all time propagation functions, measurement functions and co-variances can be written in matrix form. This is practical since the Kalman filtercalculations uses matrix inversions. If the system is not linear, it is not possibleto put the equations on matrix form. The prediction stage can still be done in nonlinear form, xk+1 = f(xk ). But the calculations of the covariance propagation, equa-tion 2.7, demands a system transpose, thus the time propagation function has to belinearized and transposed for this purpose. Same system is applicable for the nonlinear measurements. The difference between measured value and estimated valuecan be calculated using the non linear equations, but calculations of the kalmanfilter gain, equation 2.8, require matrix form and thus the measurement functionhas to be linearized.2.3.5.1 Linear and Nonlinear ConvergenceAs described in [5], chapter 4, convergence and optimality can be proven for linearfilters. In the case of the EKF neither convergence nor optimality can be proven,described in [5], chapter 5. This causes some problems since it is not possible todetermine filter convergence in any other way then to test the filter with data. How-ever this method of estimating the states of non linear systems is widely used andworks with good performance in most cases.2.3.6 Extended Kalman Filter MathematicsFor the EKF to be able to estimate the internal state of the process given a se-quence of noisy measurements the process has to be modeled with accordance tothe Kalman filter framework, specifying the functions f(ˆ k ) and h(ˆ k ) and the ma- x xtrices Qk and Rk , each time step. Where f(ˆ k ) is the system time propagation xtransfer function, h(ˆ k ) is the measurement function, Qk the system noise vari- xation matrix and Rk the measurement noise variation matrix. Given the discreetnonlinear system 2.9 and 2.10 the method for using a Kalman filter is as follows. ˆ xk+1 = f(ˆ k ) + wk x (2.9) zk = h(ˆ k ) + vk x (2.10) —page 10—
  • 19. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation ˆ ˆWhere f(ˆ k ) is the transfer function propagating the system vector xk to xk+1 , x ˆh(ˆ k ) transfers xk to the observable data zk and vk and wk is white noise originat- xing from the sensor or system model error dynamics. The estimated states in the ˆstate vector xk covariance Pk is updated for all time steps and after all measure-ments. For all time steps the linearized system time propagation transfer function ˜Fk and the linearized measurement function Hk is calculated. yk is an externalmeasurement. The Kalman filtering is done as follows.Choose initial value for the state vector x− . The initial value of the covariance ˆmatrix must be chosen so that the initial values of the states are within distance toreal value described by the covariance. Calculate Qk = E[wk wT ], Rk = E[vk vT ] k kand P− = E[(xk − x− )(xk − x− )T ] where k = 0 and xk the actual state. k ˆk ˆk1. Calculate the Kalman gain. Kk = P− HT (Hk P− HT + Rk )−1 k k k k (2.11)2. Calculate new estimate of state vector with measured values zk . xk = x− + Kk (˜ k − h(ˆ k )− ) ˆ ˆk y x (2.12)3. Calculate covariance matrix. Pk = (I − Kk Hk )P− (I − Kk Hk )T + Kk Rk KT k k (2.13)4. Propagate state vector. x− = f(ˆ k ) ˆ k+1 x (2.14)5. Project covariance matrix. P− = Fk Pk FT + Qk k+1 k (2.15)6. Calculate linearized Fk and Hk , k = k + 1, restart at point 1. —page 11—
  • 20. CHAPTER 3 Hardware Implementation3.1 The Autonomous VehicleThis thesist’ autonomous vehicle is based upon the chassis of a standard commer-cial radio controlled 1/10 scale vehicle. The vehicle is 486 mm long and 414 mmwide. Figure 3.1: The vehicle shown without the top cover.The top cover is removed and replaced by an aluminum mounting plate. Themounting plate is equipped with a PC/104 computer running Linux. The car isalso equipped with a servo control device, a camera, a GPS receiver and antenna,an inertial navigation system and a wireless LAN, all connected to the PC/104.Figure 3.2 is a schematic diagram over the components of the autonomous vehicle —page 12—
  • 21. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation Figure 3.2: Block diagram of the autonomous vehicle system.studied in this master thesis.3.2 Odometer ConstructionThere are a number of different ways to measure how far a vehicle has traveled.One is to put an encoder on one or more of the wheels of the vehicle. The encodersends a signal to a microcontroller or computer that keeps count of how far thevehicle has traveled since the counting started. If encoders have been placed onone left and one right wheel, the direction of the vehicle can also be calculated asseen in subsection 2.1.3. One error of the measurements is due to slipping of thewheels when the vehicle is accelerated fast on gravel or driving in uneven terrain.Another emanates from the fact that when the car moves in uneven terrain the tireswill compress and thus change their radius; this causes the calculated stretch ofthe wheels to differ from the real stretch, which affects both distance traveled andvehicle heading. All driven wheels always have a microslip towards the ground.In the case of the autonomous vehicle in this thesis, the wheels are constructed ina way that microslip can occur between the rim and the tire. This means that thetires might slowly turn around on the rims when the vehicle is driving. Since thetwo latter error sources are small compared to the slip caused by uneven terrain,they may be disregard. The microslip appears evenly distributed over the distancetraveled and the distance error microslip will be handled by a filter estimating thewheel radii. —page 13—
  • 22. Study of Gyro and Differential Wheelspeeds for Land Vehicle NavigationOne way to handle the slip and microslip problems is to add one or more wheelsto the car, have them springloaded towards the ground at all time and then put theencoder on these wheels. Since they are not driving there will be no micro slippageand the wheels are a lot less likely to spin. The problem with this solution is thatthe car mechanics will be more advanced and developing the system will take a lotof time. In the case of the autonomous vehicle in this master thesis, it is easier touse the standard wheels.3.2.1 Different EncodersThere are a few different types of encoders. In this section their advantages anddisadvantages will be discussed. A tachometer is a primitive part of an encoder asexplained in subsection 2.1.3.3.2.1.1 Optical TachometerThere are two different types of optical tachometers, reflex tachometers and trans-parency tachometers. Both have a light source and a light sensitive sensor. Thereflex tachometer has the light source and the light sensor close to each other onthe same side of the tachometer disc. The tachometer disc is painted with manysmall sections of black and white across the disc circumference, the black areasreflecting less light than the white areas. The light sensitive sensor registers howmuch light is reflected from the light source. Counting these high and low reflec-tions when the disc spins by the light source and sensor gives a measurement of thenumer of turns the disc has made. Figure 3.3: Reflective tachometer schematics.The transparency tachometer has the light source on one side of the disc and thelight sensitive sensor on the other. The transparency tachometer disc has holes init, thus letting light through when a hole is between the sensor and light source,and keeping the light from reaching the sensor otherwise. This way a much higherdifference between light and dark areas may be achieved. The drawback is that the —page 14—
  • 23. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigationlight source and the light sensor cannot be on the same side of the tachometer disc.This is sometimes very difficult to obtain, not even possible or just too expensive. (a) (b) Figure 3.4: Opacity encoders.Figures 3.4 (a) and (b) show optical transparency tachometers. The red plasticcomponent in the figure to the left is the light source and the white transparentcomponent is the light sensitive sensor. Between them the tachometer disc can beseen with its slots where the light is let trough.The largest drawback for both of these tachometers is dirt. When dirt or dust getsstuck on the black and white areas of the reflective wheel or in the holes of thetransparency encoder the readings will be corrupt. Since the thesist’ autonomousvehicle will be moving outdoors, dirt will be a big problem for these sensors.3.2.1.2 Magnetic TachometerIf the light and dark areas of the optical tachometers are exchanged for magneticnorth and south poles, a magnetic field sensor can be used to sense the change inmagnetic field instead of light intensity. One sensor that can sense magnetic fieldis the Hall Effect sensor. The Hall Effect sensors can be placed at a distance fromthe magnets and the magnetic field will not be disturbed by dirt or dust. Hall Effectsensors might be a little slower than the optical ones.3.2.1.3 From Tachometer to EncoderA tachometer only counts the transitions between light and dark, or between mag-netic north and south poles, but will not tell which direction the wheel is turning.By adding another sensor placed half a count away from the first one the directionof the rotation can also be derived as explained in subsection 2.1.3.The signal from the first sensor is called channel A and the second channel B. Forsome applications one more channel is needed. This channel is called I (for index —page 15—
  • 24. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigationchannel). The index channel has only one count per lap and may be placed any-where in the tachometer disc. This is useful both when the system is first turnedon, and a certain position of the wheel needs to be found, and when for some rea-son the encoder misses a count, which can be corrected by the index channel. Thiscorrection is done by keeping track of how many ticks there are on a lap, and howmany has been counted since the last time the index was counted. If for instancenine positive counts has been made since the last index, and there should be ten ina lap, the electronics know that one tick was missed and this can be corrected byadding one positive count.3.3 The Encoder SolutionThere are three parts that has to be done to make an encoder for the autonomousvehicle in this thesis. First, the mechanics has to be made considering the environ-ment in which it will be working. Second, the electronics must be able to transformthe signals from the sensor so that the micro controller can count the transitions.The micro controller must also be fast enough not to miss any of the transitions andat the same time handle the serial communication to the vehicle onboard computer.Third, the software has to be efficient enough that the limited computational powerof the micro controller can handle it.3.3.1 Encoder MechanicsBecause of outdoor application of the thesis autonomous vehicle the optical en-coders are dismissed. The dust problem is considered too big and magnetic en-coders are well suited for dusty environments.Neodymium magnets are chosen for their magnetic strength. Higher magneticstrength means higher signal-noise ratio and that the Hall Effect sensors are al-lowed to be placed further away from the magnets. The magnets are placed be-tween the rim and the tire where they are protected from moist and dirt. Neodymiumis very corrosive and protection from moist and dirt is needed.The neodymium magnets are fixed to the rims using a system of thin lamellas. Toget high enough precision together with low weight the lamellas are made usinga water cut machine cutting the lamellas from 1.2mm thick plastic sheets. Watercutting machines have about five hundreds of a millimeter of precision, which isconsidered to be high enough. At both ends of the lamellas one more lamella isplaced to keep the magnets from moving coaxially.Keeping the weight low is needed since the sensors are placed at the wheel ofthe vehicle, and unspringed weight should be kept as low as possibly to reduce theenergy of that mass when moving over uneven terrain.The lamellas are placed as far as possible towards the inner side of the wheels —page 16—
  • 25. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation Figure 3.5: Encoder wheel assembly.so that the sensors are easy to place on the other side of the rims. The magnets areplaced with every other magnet having its magnetic north pole facing outwards,and every other inwards. The lamellas are fixed using screws to hold the lamellastogether with the magnets in their slots. The lamellas are glued to the ribs using atwo component glue.3.3.2 Encoder ElectronicsThe purpose of the encoder electronics is to count the transitions of magnetic fieldsensed by the Hall Effect sensors and send the number of transitions done during asample period to the vehicle onboard PC/104. The sensors are placed half a magnetwidth from each other, so the pulse wave from the magnets were 90 degrees shiftedas explained in subsection 2.1.3.Four different Hall Effect sensors were tested. Two digital (Hall Effect switches)and two analog. The digital sensors had a schmitt trigger function which wouldmake the square waves a little different when the wheel is turning clockwise andwhen it is turning counterclockwise, they were therefore dismissed. The two ana-log sensors were Allegrot’s A3515 and A3518. A3515 has a maximum sensitivityof 10 G and A3518 20 G. Both sensors were tested with the magnetic encoder discand it proved that neodymium magnets were strong enough to saturate both sensorsbut A3518 had, because of its longer sensitivity range, a little smoother transitionbetween high pulse and low pulse. Therefore A3518 was chosen. The sensors werefastened onto the wheel suspension system close to the rim.The analogue signal generated by the Hall Effect sensors connected to connec-tor SV1, is fed into a comparator (IC4A-IC7A and IC4B-IC7B in figure 3.6), sothat any positive magnetic field sensed generates a logic 1 (5 volt), and any neg- —page 17—
  • 26. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation Figure 3.6: Encoder wheel assembly.ative magnetic field sensed generates a logic 0 (0 volt). These logic level squarewaves are suitable to be counted by a micro controller (IC3).There are a number of different micro controllers on the market. Microchip Tech-nology delivers a micro controller called PIC, Motorola has their 68HC-series,Ubicom has a series of SX chips and Atmel has their AVR series. To handle thecounting a Atmel AVR micro controller is chosen. The Atmel series of 8-bit AVRcontrollers are a modern solution containing all the features needed for this task.The AVR-series are also easy to program using C-code and the programming en-vironment CodeVisionAVR delivered by Hp InfoTech.Each of the four square wave signals, two from each rear wheel, is fed into theAVR micro controller using four interrupt driven inputs. Each time one of thesignals change state a certain code is run in the micro controller to decide whichchannel changed and if the count to make is positive or negative.When moving at slow velocities the sensors bounce a few times before settlingat the new state. The expression "bounce" means that the signal changes from onestate to the other a few times back and forth before settling at the new state. Ifthe amount of bounces is too high the micro controller will not be able to countall the transitions and thereby fail to count correctly. To reduce bouncing a 2.2µFcapacitor (C3-C6 in figure 3.6) is added between the analog signal and ground tosmooth the Hall Effect sensor output. When this is done almost all of the bouncingdisappears, and for higher velocity there is no bouncing at all. —page 18—
  • 27. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation S1 = 0 S2 = 0 S1 = 0 S2 = 1 S1 = 1 S2 = 1 S1 = 1 S2 = 0 P1 = 0 P2 = 0 X + X - P1 = 0 P2 = 1 - X + X P1 = 1 P2 = 1 X - X + P1 = 1 P2 = 0 + X - X Table 3.1: Karnaugh map for software comparison minimization.3.3.3 Micro Controller SoftwareThe purpose of the software is to keep track of the number of passing magneticfield transitions and at certain time intervals deliver them to the onboard PC/104.Since there are four inputs that can change transition, there are 16 different statesthat the input could have. Binary 0000, 0001, 0010, and so on. To know if oneshould be added or subtracted, the prior state has to be remembered. From eachwheel there are only two prior states from which a transition to the present statecan be made. If the program is split up into two parts, one for each wheel sensorpair, then each sensor has four states that each can be transitioned to from two priorstates. That means that eight comparisons have to be made for each sensor pair toderive if it is an positive or negative transition, 16 totals for both wheels. Even thouthe electronic is very fast the bouncing that is still present even when the capaci-tors are used might cause problems. To minimize the risk of this problem time tocalculate a transition has to be as short as possible. Not all combinations of the twobits of new sensor data and two bits of prior sample data is possible, they can onlymake transitions from 1 to 2 to 3 to 4 to 1 again, or the opposite direction. A jumpfrom 2 to 4 can never be done. Therefore a minimizing of the comparisons is done.The transitions are fed into a Karnaugh map, table 3.1. + marks if the transitionshould lead to an increase of the number of transitions, - marks the decrease and Xmarks an illegal change. Note that no change is seen as an illegal change since ifno change has been made, the comparison code in the micro controller is not run.S1 and S2 are the present state of the sensors and P1 and P2 are the prior state.According to the Karnaugh map minimizing rules [6] the states were grouped to-gether four and four in a square. Each containing only + and X or - and X. Giventhe Karnaugh map rules the only comparison that has to be made is between S1 andP2 . S1 = P2 produces an increase of transitions and S1 = P2 a decrease. The microcontroller now only has to do two times two comparisons instead of the earlier twotimes eight comparisons, effectively now cutting the computation time in four.The calculations of the transitions is set to be done during the fix time period of 20.000.000about 19 Hz ( 1024∗256∗4 Hz). The numbers of calculated transitions are then sentvia the RS-232 serial link to the PC-104 and the two transition counters are resetto zero and then start calculating again. —page 19—
  • 28. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation3.3.4 Sensor Error AnalysisThere are a few different errors of the odometer sensor. Quantization, nonlinearityand missed tick are the most prominent.Quantization error is the largest right before and right after a transition. If thequantization step is from one to two, and the transition between one and two hap-pens at 1.5, then the absolute quantization error is 0.5 just before and just after thetransition. For the wheel encoder this quantization is about 0.16 radians thus thelargest quantization error is 0.08 radians.For movement forward in higher velocities this does not matter much since 0.08 ra-dians to little during one sample will be 0.08 radians to many the next. Where thisquantization does cause problem is during slight turns. If one wheel should makefor i.e. 10 ticks the other should make for i.e. 10.1, thus for nine samples measur-ing 10 ticks and for one sample 11 ticks. The quantization error does not cause thedistance the vehicle traveled or the heading of the vehicle to differ from the realvalue over time, but it does affect the estimated position. This since the positiondepends on that heading and velocity are correlated, which they are not. Quan-tization error for heading at one time is corrected later, but if the vehicle movesduring this error time the calculation for position change will be in slightly wrongdirection.Linearity error is very small since the mounting for the magnets inside the rimswere water cut with a precision of about of about 2 micrometers. The error is thus3 × 10−5 radians and can be neglected compared to quantization error. This errordoes not increase by time.If the wheel is spinning too fast, either the micro controller or the comparatormight not be able handle the frequency of transitions from the encoder. The wheelradius is about 72 cm so the circumference of the wheel is about 0.45 meters. If thevehicle is traveling at 100 km/h, which is a lot higher than any reasonable veloc-ity for this autonomous vehicle, the wheel will turn less than 65 times per second.With 40 pulses per lap, 2.600 ticks per wheel and second has to be handled. That’s5.200 transitions for the micro controller to count using both encoders. Every tickuses two if statements each being about 20 instructions for the micro controllerwhich is 208.000 instructions totally. The micro controller computes 20 millioninstructions per second so the margin is about 100 times.Each of the four comparators handle 1.300 pulses. According to the compara-tor datasheet the comparators are made to work for frequencies of up to 1.1 MHz.That’s a 800 times margin.The quantization error does cause the position to drift over time and this is takeninto account when setting the odometry noise. The nonlinearity is a lot smallerthan the quantization error and is disregarded. The electronics with 100 times mar-gin and the comparators with 800 times margin will have no trouble handling thecounting of ticks from the encoders for any realistic velocity of the autonomous —page 20—
  • 29. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigationvehicle studied in this thesis. —page 21—
  • 30. CHAPTER 4 Kalman Filter ImplementationIn this chapter the different navigation filters and their performance will be de-scribed briefly. In chapter 5 the filters are evaluated in more detail. In this thesisa flat earth representation is used providing navigation states expressed in east andnorth coordinates. The third dimension, up, is disregarded. Figure 4.1: Parameter definitions.The heading angle of the vehicle is defined from north to west, since some of thefilters have been copied from [4], in which this definition is used.The system equations for all the systems are on the form d x = f(x,t) + g(x,t)w(t), (4.1) dt y = h(x,t) + v(t), (4.2) —page 22—
  • 31. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigationwhere x is the system states, f(x,t) is the system function, g(x,t) is the noise distri-bution for the system function, w(t) is white noise, y is the system output, h(x,t) isthe measurement function and v(t) is the measurement noise. In the sequel, theseequations will be given without the time argument t.Two filters use only odometry and GPS. The first estimates the wheel radii, po-sition and heading and is described in section 4.2. The second estimates the wheelradii, position, heading and rear wheel axis width and is described in section 4.3.Both are found to work well when the GPS data is present, but have some troubleto find a good estimates for the wheel radii and the rear wheel axis width. WhenGPS data is not present, resulting in position and heading drift.In order to overcome this problem, a filter is tested where a heading gyro predictingthe vehicle heading is used. This filter is described in section 4.4. The odometerheading information is disregarded in this system. This filter is designed to estimateposition, heading, gyro bias and gyro scale factor and uses the odometry velocity.This system gives slightly better results than the GPS-odometry integration but itstill have quite a large drift. This drift, however, does not have the same featuresas the odometer drift. The odometry has a steady drift towards one direction whilethe gyro seems to have a more random drift back and forth.A filter using the external measurements of the vehicle heading angular rate fromboth the odometry and gyro is developed in subsection 4.5. The vehicle estimatedheading angular rate is there made to drive the system heading. For a measurementfunction for the odometry to be determined, the wheel radius error instead of thewheel radius it self has to be estimated. Thus an initial guess of the wheel radiushas to be made, from which the wheel radius errors are estimated. For this systemthe position, heading, heading angular rate, wheel radius error and gyro bias is es-timated. This filter has some problems with the system noise specification for theheading angular rate since the odometer and gyro measurements do not execute atthe same time. However, for a certain value of noise the system runs smoothly alsoduring GPS outages.For all the filters, the vehicle ground velocity v is obtainable at all time steps usingequation 2.3, which is repeated as equation 4.3 for convenience. ωr Rr + ωl Rl v= (4.3) 2For each filter the linear observability is analyzed. For the Kalman filter to be ableto estimate the system states all the states have to be observable. To prove observ-ability, a nonlinear observability analysis has to be done. Since the mathematicsfor nonlinear observability is very complex this is not done in this thesis. Linearobservability does not prove nonlinear observability, but still gives a hint about theobservability of the system at least around the linearization point.Linear observability is made by looking at the Jacobian matrices H and F of themeasurement function h and the system functions f , respectively, and construct the —page 23—
  • 32. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigationobservability matrix as described in equation 4.4.   H  HF     . . , (4.4)  .  HF k−1where k is large enough to get a matrix of full rank, if possible. If there is no k largeenough to give the observability matrix full rank the system is not observable.4.1 Filter and System Variable Definitions.Many of the system and filter variables are the same for all of the filters. Some areonly used in one or two filters. All variables are defined here. —page 24—
  • 33. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation E The east position. ˜ E The east position measurements from the GPS. N The north position. ˜ N The north position measurements from the GPS. ψ The estimated vehicle heading angle from north to west ˜ ψ The GPS heading calculated using the GPS measurement vEGPS ˜ and vNGPS of the velocity components and equation 4.5. For veloc- ˜ ˜ ities lower than 0.5 m/s the ψ is disregarded because of its inaccu- racy. vGPS ˜ the GPS velocity calculated from the GPS measurement vEGPS and ˜ vNGPS of the velocity components and equation 4.6. ˜ Rr The estimated right wheel radius. δRrˆ The right wheel radius deviation from hand measured value. ˜ r + δRr R ˆ The real right wheel radius. Rl The estimated left wheel radius. δRlˆ The left wheel radius deviation from hand measured value. ˜ l + δRl R ˆ The real left wheel radius ˜ ωr The measurements of the right wheel angular rate using the en- coders. ˜ ωl The measurements of the left wheel angular rate using the en- coders. B The rear wheel axis width. ˜ ˆ ˜ ˆ v ˜ The measured velocity ωr Rr +ωl Rl derived from the odometer and 2 the wheel radii. r The real vehicle angular rate. r ˜ The gyro measured vehicle angular rate. b The gyro bias. s The gyro scale factor. vgyro (t) The gyro angular rate noise as described in section 4.6. vEGPS The GPS east position measurement noise as described in section 4.6. vNGPS The GPS north position measurement noise as described in section 4.6. vψGPS The GPS heading noise as described in section 4.6. vvGPS The GPS heading noise as described in section 4.6. vodometry (t) The odometry speed noise as described in section 4.6. wEodometry The odometry east position system noise whose characteristics is described in section 4.6. wNodometry The odometry north potition system noise whose characteristics is described in section 4.6. wψodometry The odometry heading system noise whose characteristics is de- scribed in section 4.6. wψgyro The gyro heading system noise whose characteristics are described in section 4.6. vEGPS ˜ ˜ ψ = arctan2( ) (4.5) vNGPS ˜ —page 25—
  • 34. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation vGPS = ˜ v2 GPS + v2 GPS ˜E ˜N (4.6)Where arctan2 is the four quadrant version of arctan. The inverse trigonometricfunction arctan2(y, x) is defined as r = arctan(y/x) using:if x = y = 0, then r = inde f initeif x > 0 and y = 0, then r = 0if x < 0 and y = 0, then r = π, elseif y < 0, then −π < r < 0if y > 0, then 0 < r < πThe function is used to find the direction from one point to another in 2-dimensionEuclidean space.4.2 Odometry-GPS Estimating Wheel Radii.This model is taken, somewhat modified, from [4].In this system only the GPS and the odometry is used. The odometry is usedin the Kalman filter prediction step and the GPS serves as measurements. Position,heading and the wheel radii are estimated in the filter. The system is as in equation4.7.    ˜ ˜    E − ωr Rr +ωl Rr sin(ψ) 2 wEodometry  ˜ ˜   ωr Rr +ωl Rr cos(ψ)   wNodometry  N d    = 2 ˜ ˜ ωr Rr −ωl Rr    +  wψ   ψ (4.7) dt       B     odometry   Rr 0 0 Rl 0 0The state vector is denoted x1 . The east and north position, angle between thenorth axis and the velocity vector and ground velocity is used as measurements.The measurement function h1 (x1 ) is described with its noise in the filter outputfunction in equation 4.8.       ˜ E E vEGPS  N ˜  N   vNGPS  y1 =  ˜   ψ  = h1 (x1 ) + v(t) =   +    vψGPS  . (4.8) ˜ ψ ˜ ˜ ωr Rr +ωl Rl vGPS ˜ 2 vvGPSThe prediction step is performed by solving the nonlinear equation 4.7 without thenoise term by using Euler integration (Euler integration is assumed sufficient sincethe dynamics is relatively low and the update rate high). The covariance matrixis propagated using the Ricatti equation whence the system function has to be lin-earized according to equation 4.9. —page 26—
  • 35. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation  ˜ ˆ ˜ ˆ  −ωr sin ψ −ωl sin ψ 0 ˜ ˆ 0 −v cos ψ 2 2  ˜ ωr cos ψˆ ˜ ωl cos ψˆ  ˆ ∂f1  0 0 −v sin ψ ˜ 2 ˜ 2 ˜   F1 = = 0 0 0 ωr − ωl , (4.9) ∂ˆ 1  x B B   0 0 0 0 0  0 0 0 0 0Further, the measurement function h1 is already linear and its Jacobian matrix is   1 0 0 0 0 ∂h1  0 1 0 0 0  H1 = = x1 =  ˜  0 0 1 0 0 .  (4.10) ∂ˆx ˜ ωr ˜ ωl 0 0 0 2 24.2.1 ObservabilityFor the filter in this section, a k of 2 is enough to get full rank under some circum-stances. The observability matrix is as equation 4.11.   1 0 0 0 0  0 1 0 0 0     0 0 1 0 0     0 0 0 ˜ ωr ˜ ωl  H1  2 2  = ˜ ˆ ˜ ˆ  (4.11) H1 F1  0 0 −v cos ψ − ωr sin ψ ˜ ˆ 2 − ωl sin ψ 2   ˜ ˆ ˜ ˆ    0 0 −v sin ψ ωr cos ψ ˜ ˆ 2 ωl cos ψ 2   ˜ ˜l  0 0 0 ωr B −ω B  0 0 0 0 0As seen in equation 4.11 a matrix of full rank can be constructed using rows onethrough four and row seven. The rest of the rows are not contributing to the matrix ˜ ˜rank. One observations done is that if ωr and/or ωl are zero the matrix does nothave full rank thus is not observable. ˜ ˜The observability analysis result is that if either ωr = 0 and/or ωl = 0, the sys- ˜tem is not observable for this k. Otherwise the system is observable. ωr = 0 andω˜ l = 0 corresponds to the vehicle having no velocity meaning that the two wheelradii cannot be estimated.4.3 Odometry-GPS Estimating Wheel Radii and Rear Wheel Axis WidthThis model is taken, somewhat modified, from [4]. —page 27—
  • 36. Study of Gyro and Differential Wheelspeeds for Land Vehicle NavigationIn this system the GPS and the odometry is used. The odometry is used in theKalman filter prediction step and the GPS serves as measurements. Position, head-ing, wheel radii and the rear wheel base width is estimated in the filter. The systemis as equation 4.12.    ˜ ˜    E − ωr Rr +ωl Rl sin(ψ) 2 wEodometry  N   ωr Rr +ωl Rl cos(ψ)   wNodometry ˜ ˜     2    d  ψ    ˜ ˜ ωr Rr −ωl Rl     wψodometry   = ˆ B +  (4.12) dt   Rr     0   0         Rl 0  0 B 0 0The estimated state vector is denoted x2 . The east and north position, angle be-tween the north axis and the velocity vector and ground velocity is used as mea-surements. The measurement function h2 (x2 ) is described with its noise in thefilter output function in equation 4.13.       ˜ E E vEGPS  ˜ N   N   vNGPS  y2 (ˆ ) =  ˜ x   = h2 (x2 ) + v(t) =    +    vψGPS  (4.13) ˜ ψ ψ ˜ ˜ ωr Rr +ωl Rl vGPS ˜ 2 vvGPSThe prediction step is performed by solving the nonlinear equation 4.12 withoutthe noise term by using Euler integration (Euler integration is assumed sufficientsince the dynamics is relatively low and the update rate high). The covariance ma-trix is propagated using the Ricatti equation whence the system function has to belinearized according to equation 4.14.  ˜ −ωr sin ψˆ ˜ −ωl sin ψˆ  0 ˜ ˆ 0 −v cos ψ 2 2 0  0 ˆ 0 −v sin ψ ˜ ˜ ωr cos ψˆ ˜ ωl cos ψˆ 0   2 2  ∂f2  0 0 0 ˜ ωr ˜ − ωˆl ˜ ˆ −˜ ˆ − ωr RrB2ωl Rl   F2 = = Bˆ B ˆ  (4.14) ∂ˆ 2  x 0 0 0 0 0 0     0 0 0 0 0 0  0 0 0 0 0 0Further, the measurement function h2 is linearized and its Jacobian matrix is   1 0 0 0 0 0 ∂h2  0 1 0 0 0 0  H2 = =  (4.15) ∂ˆ 2  0 x 0 1 0 0 0  ˜ ωr ˜ ωl 0 0 0 2 2 0 —page 28—
  • 37. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation4.3.1 ObservabilityFor the filter in this section, a k of 2 is enough to get full rank under some circum-stances. The observability matrix is as equation 4.16.   1 0 0 0 0 0  0 1 0 0 0 0     0 0 1 0 0 0     0 0 0 ˜ ωr ˜ ωl 0  H2  2 2  = ˜ ˆ ˜ ˆ  (4.16) H2 F2  0 0 −v cos ψ − ωr sin ψ ˜ ˆ 2 − ωl sin ψ 2 0   ˜ ˆ ˜ ˆ   0 0 −v sin ψ ωr cos ψ ˜ ˆ ωl cos ψ 0   2 2 ˜ ˜ ˆ−˜ ˆ   0 0 0 ˜ ωr − ωˆl − ωr RrB2ωl Rl  Bˆ B ˆ 0 0 0 0 0 0As seen in equation 4.16 all the columns are independent, thus the matrix has full ˜ ˜rank. One observations that is done, is that if ωr and/or ωl are zero the matrix does ˜ ˆ ˜ ˆnot have full rank thus is not observable. If ωr Rr = ωl Rl , then row seven, columnsix is zero. Since this is the only term in column six the system is not observablein this chase. ˜ ˜ ˜ ˆ ˜ ˆThe observability analysis result is that if either ωr = 0, ωl = 0 or ωr Rr = ωl Rl thesystem is not observable for this k. Otherwise the system is observable. ωr = 0˜ ˜and ωl = 0 corresponds to the vehicle having no velocity meaning that the two ˜ ˆ ˜ ˆwheel radius cannot be estimated. If ωr Rr = ωl Rl the vehicle is not turning, thusthe vehicle rear wheel axis width does not matter for the position or velocity of thevehicle, it is not observable.4.4 Odometry-Gyro-GPS estimating Gyro Bias and Scale FactorThis model is taken, somewhat modified, from [4].In this system the GPS, the gyro and the odometry is used. In the Kalman filterprediction stage the odometry is used for vehicle velocity and the gyro for vehicleheading and the GPS serves as measurements. Position, heading, gyro bias andgyro scale factor is estimated. The gyro measurement model chosen is as equation4.17. r(t) − b r(t) = ˜ + v1 (t) (4.17) s —page 29—
  • 38. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation4.17 is a variable substitution from 4.18 r1 (t) = s1 r(t) + b1 + v2 (t) ˜ (4.18)made to avoid nonlinearities in the covariance matrix 4.21 for the correspondingelements. In the chase of this system the white noise v1 (t) becomes system noisewψgyro . This gives the system given in equation 4.19.    ˜ ˜    E − ωr Rr +ωl Rl sin(ψ) 2 wEodometry  N   ˜ r Rr +ωl Rl ω ˜ cos(ψ)   wNodometry  d    = 2   +  , ψ −˜s − b r wψgyro (4.19) dt             b 0 0 s 0 0ψ is here the vehicle heading angle from north to west obtained from the gyro witha negative sign. This because the gyro angular rate is defined from north to westand the system heading from north to east. s and b is modeled as random constants.The estimated state vector is denoted x3 . The east and north position and the anglebetween the north axis is used as measurements. The measurement function h3 (x3 )is described with its noise in the filter output function in equation 4.20.       ˜ E E vEGPS y3 =  N  = h3 (x3 ) + v(t) =  N  +  vNGPS  ˜ ˜ (4.20) ˜ ψGPS ψ vψGPSThe prediction step is performed by solving the nonlinear equation 4.19 withoutthe noise term by using Euler integration (Euler integration is assumed sufficientsince the dynamics is relatively low and the update rate high). The covariance ma-trix is propagated using the Ricatti equation whence the system function has to belinearized according to equation 4.21.   0 ˜ 0 −v cos ψ 0 ˜ 0  0 ˜ 0 −v sin ψ 0 ˜ 0  ∂f3   F3 = ˜3 =  =x  0 0 0 −1 −rgyro ,  (4.21) ∂ˆ 3 x   0 0 0 0 0 0 0 0 0 0Further, the measurement function h2 is already linear and its Jacobian matrix is   1 0 0 0 0 ∂h3  H3 = = 0 1 0 0 0 . (4.22) ∂ˆ 3 x 0 0 1 0 0 —page 30—
  • 39. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation4.4.1 ObservabilityFor the filter in this section there is no k large enough to get full rank. All F3k−1 fork larger than 3 are zero matrices. The observability matrix is as equation 4.23 withrows seven, eight and eleven though thirteen not printed out since they are rows ofzeros and do not contribute to matrix rank.   1 0 0 0 0  0 1 0 0 0     0 0 1 0 0    H3  0 ˆ 0 −v cos ψ ˜ 0 0  =    (4.23) H3 F3  0 ˜ ˆ 0 −v sin ψ 0 0   0 0 0 −1 −˜gyro r     0 0 0 ˆ ˜ ˆ v cos ψ v cos ψrgyro ˜  0 0 0 ˆ ˜ ˆ v sin ψ v sin ψrgyro ˜Column four and five are linearly dependent, so are row three through five. Thisgive the system rank of five where six would have been full rank. The linear sys-tem is not observable and this system will probably have difficulties estimating itsstates. It must be noted thou that for the linear system to be non observable doesnot prove that the nonlinear system is non observable, its just an indication of it.The observability analysis result is that the linearization of this system is not ob-servable and this will probably cause the system to have problems estimating itsstates.4.5 Odometry-Gyro-GPS Estimating Wheel Radiuses Er- ror and Gyro BiasThis model is taken, somewhat modified, from [4].In this system the GPS, the gyro and the odometry are used. In the Kalman filterprediction stage the odometry is used for vehicle velocity. The GPS, the odometryand the gyro serve as measurements where the gyro and the odometry measuresthe vehicle heading angular rate. Position, heading, heading angular rate, wheelradius error and gyro bias are estimated. The gyro measurement model chosen isdescribed as rgyro (t) = r(t) + b + vgyro (t), ˜ (4.24)The odometer angular rate measurement model chosen is described as ˜ ˆ ˜ ˆ ωr δRr − ωl δRl rodometry (t) = r(t) + ˜ + vodometry (t), (4.25) B —page 31—
  • 40. Study of Gyro and Differential Wheelspeeds for Land Vehicle NavigationThis gives the system as equation 4.26.    ˜ ˜ ˜ ˜    E − ωr (Rr +δRr )+ωl (Rl +δRl ) sin ψ 2 wEodometry  N   ˜ ˜ ˜ ˜ − ωr (Rr +δRr )+ωl (Rl +δRl ) cos ψ   wNodometry         ψ   2   wψr  d    r     r =   + wr  (4.26) dt    0   δRr         0   0   δRl       0  0 b 0 0The estimated state vector is denoted x4 . The east and north position, ground ve-locity and the angle between the north axis is used as measurements. The measure-ment function h3 (x4 ) is described with its noise in the filter output function. Sincethese three measurements does come from three different sensors that do not havethe same sample rate, the measurements cannot be done within one measurementfunction. Instead they have to be written separately like equations 4.27, 4.28 and4.29.       ˜ E E vEGPS  ˜ N   N   vNGPS y4.1 = ˜   = h4.1 (x4 ) + vGPS (t) =    +   vψGPS   ˜ ψ ψ ˜ ˜ ωr (Rr +δRr )+ωl (Rl +δRl ) vGPS ˜ 2 vvGPS (4.27) ˜ y4.2 = rgyro ˜ , h4.2 + vgyro (t) = [r(t) + b] + vrGyro (4.28) ˜ ˆ ˜ ˆ ωr δRr − ωl δRl ˜ y4.3 = rodometry ˜ , h4.3 + vodometry (t) = r(t) + + vrOdometry B (4.29) vEGPS ˜ ˜ ψ = arctan2( ) (4.30) vNGPS ˜ vGPS = ˜ v2 GPS + v2 GPS ˜E ˜N (4.31) —page 32—
  • 41. Study of Gyro and Differential Wheelspeeds for Land Vehicle NavigationThe prediction step is performed by solving the nonlinear equation 4.26 by usingEuler integration (Euler integration is assumed sufficient since the dynamics is rel-atively low and the update rate high). The covariance matrix is propagated usingthe Ricatti equation whence the system function has to be linearized according toequation 4.32.  ˜ ˆ ˜ ˆ  0 ˆ 0 −v cos ψ ˜ 0 − ωr sin ψ 2 − ωl sin ψ 2 0  ˜ ˆ ˜ ˆ   0 ˜ ˆ 0 −v sin ψ 0 ωr cos ψ 2 ωl cos ψ 2 0    ∂f4  0 0 0 1 0 0 0   F4 = = 0 0 0 0 0 0 0 , (4.32) ∂ˆ 4  x   0 0 0 0 0 0 0     0 0 0 0 0 0 0  0 0 0 0 0 0 0Further the measurements are already linear and can be written on matrix form as4.33, 4.34 and 4.35.Further, the measurement function h4 .1, h4 .2 and h4 .3 are already linear and theirJacobian matrices are   1 0 0 0 0 0 0 ∂h4.1  0 1 0 0 0 0 0  H4.1 = =  0 , (4.33) ∂ˆ 4 x 0 1 0 0 0 0  ˜ ωr ˜ ωl 0 0 0 0 2 2 0 ∂h4.2 H4.2 = = 0 0 0 1 0 0 1 , (4.34) ∂ˆ 4 x ∂h4.3 ˜ ˜ H4.3 = = 0 0 0 1 ωr B − ωl B 0 . (4.35) ∂ˆ 4 x4.5.1 ObservabilityHere H is as equation 4.36.   H4.1 H4 =  H4.2  (4.36) H4.3For the filter in this section, a k of 2 is enough to get full rank under some circum-stances. The observability matrix is as equation 4.37. —page 33—
  • 42. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation   1 0 0 0 0 0 0  0 1 0 0 0 0 0     0 0 1 0 0 0 0     0 0 0 0 ˜ ωr ˜ ωl 0   2 2   0 0 0 1 0 0 1     0 0 0 1 ˜ ωr ˜ − ωr 0   B B  (4.37)  ˜ ˆ ˆ   0 ˆ − ωr sin ψ 0 −v cos ψ ˜ − 2ωl sin ψ 0 0   ˜ 2 ˆ ˜ ˆ    0 0 −v sin ψ ωr cos ψ ˜ ˆ 2 ωl cos ψ 2 0 0    0 0 0 1 0 0 0     0 0 0 0 0 0 0  0 0 0 0 0 0 0If rows one through six and row nine are used, the matrix has full rank unless ωrand ωl are zero, then the matrix does not have full rank since row four, and sixthough eight is zero. This is the only condition for observability for this system aslong as all measurements are done. If the GPS data is unavailable the system is notobservable and the estimated parameters might drift away.The observability analysis result is that the system is observable as long as ωr andωl are not zero. Since this is the only condition for observability and that the ob-servability matrix, is a diagonal matrix with the exception for the wheel radius, thissystem will probably preform well in the estimations of its states.4.6 System and Measurement Noises Q and RA Kalman filter requires knowledge about the process noise covariance Qk = cov(wk )and the measurement noise covariance Rk = cov(vk ). The system noise matrix Qktells how much the system should be trusted and the measurement noise matrix Rkdecides how much the measurements should be trusted. Qk is related to the noiseand measurements used to drive the system equations while Rk is related to themeasurements made using sensors to correct the drift of the system states causedby system model errors and errors in the measurements from the sensors drivingthe system. These noise levels can be derived from how the sensors and the systemwork and are calculated as follows.4.6.1 System Noise4.6.1.1 Wheel Radius NoiseThe wheel radii are modeled as random constants and thus the corresponding sys-tem noise is zero. —page 34—
  • 43. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation4.6.1.2 Rear Wheel Axis Width NoiseThe rear wheel axis width is modeled as a random constant and thus the systemnoise is zero.4.6.1.3 Odometer Position NoiseThe odometer presents the angular turn of the wheel with an error smaller than halfa tick. Half a tick corresponds to 0.08 radians since there are 40 ticks in a lap. Theangle velocity measurement errors va and vb are thus limited by 0.08 rad/s. Thenoise for east position is, va Rr + vb Rl wEodometry = cos(ψ), (4.38) 2taken from 2.5 and 4.7. A cosine is at most one and setting the cosine term to onesimplifies the calculations. Rr and Rl is seen as constans at the measured value0.072 m. This gives the odometer position noise variance 0.082 0.0722 + 0.082 0.0722 σ2 odometry =E w2 odometry = E E = 0.004m (4.39) 4The same value for the north noise variance is used, i.e. E w2 odometry =σ2 odometry = E Eσ2 odometry . Consequently, the standard deviation used for the noises in east and north Ndirection are, σNodometry = σEodometry = 1.66 × 10−5 m. (4.40)4.6.1.4 Odometer Angle NoiseThe odometer presents the angular turn of the wheel with less than half a tickwrong. Half a tick corresponds to 0.08 radians. The noise equation for odometerheading position is Vc Rr + vd Rl wψodometry = , (4.41) Btaken from 2.6 and 4.7. Also the measurement errors vc and vd are limited by 0.08rad/s. Rr and Rl can be seen as constants at the measured value 0.072 m. B isalso measured constant with the value 0.41 m. This gives the odometer headingstandard deviation 0.0064 ∗ 0.0722 + 0.0064 ∗ 0.0722 σψodometry = = 0.02rad (4.42) 0.4124.6.1.5 Heading Angular VelocityIf a slight turn is done during the time of a few samples, the gyro divides the totalturn over the few samples, but the odometer, that because of its quantization canonly show for i.e. zero rad/s or 0.5 rad/s, has to accumulate its angular rate during afew samples. If the gyro shows 0.1 rad/s, the odometer will show zero rad/s for four —page 35—
  • 44. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigationsamples, and then for the last sample show 0.5 rad/s. If the system heading angularrate noise is set high, the angular rate from the gyro will set the heading angularrate to around 0.1 rad/s and the odometer will for four of the samples set it aroundzero rad/s and the last sample set it to around 0.5 rad/s. Setting the system noise toolow will hinder the measurements to affect the estimated heading velocity as muchas it should to be able to estimate the true heading angular velocity. A numberof different noise levels were tested and it was found that the system was verysensitive to a too high or too low system heading angular velocity noise. A noiselevel of 5 rad/s proved to give good results. This gives the heading angular velocitystandard deviation σr = 5rad/s.4.6.1.6 Rate Gyro NoiseThe gyro noise is given in the Bulmer MICRO-ISU BP3010 datasheet as 0.004rad/s RMS [7]. Thus the process variance wψgyro in 4.19 has the standard deviation4e−3 rad/s.4.6.2 Measurement NoiseThe GPS noise levels are taken from the GPS SuperStar II manual.4.6.2.1 GPS Position Noise ˜ ˜Suppose the measurements E and N of the east and north positions E and N, arerelated as, ˜ E = E +VEGPS , (4.43) ˜ N = N +VNGPS . (4.44)The position noises vEGPS and vNGPS for the GPS is given as less than five metersCEP which means that 50% of the measurements are made within a circle of five 2meter radius. The variance is calculated from CEP using the formula 1.2CEP [8] √ 2and is the same in both coordinates. Thus E v2 GPS = σ2 GPS = E v2 GPS = σ2 GPS = N N E E 2 1.2∗5 √ 2 and the standard deviations are σNGPS = σEGPS = 4.24 m.4.6.2.2 GPS Velocity NoiseThe GPS velocity noise is given in the GPS data sheet [9] as 0.05 m/s RMS thusthe velocity standard deviation σvGPS = 0.05 m/s.4.6.2.3 GPS Heading NoiseCall the velocity components in E and N direction vE and vn , respectively, andsuppose that the GPS gives the corresponding measurements —page 36—
  • 45. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation vE = vE + ∆vE , ˜ (4.45) vN = vN + ∆vN , ˜ (4.46)where ∆vE and ∆vN are the measurement errors. The corresponding error in head-ing angle is then −vE − ∆vE ˜ −vE vE ∆vN − vN ∆vE ˜ ∆ψ=ψ − ψ = arctan − arctan = (4.47) vN + ∆vn ˜ vN v2to first order, where v2 = v2 + v2 . The variances for the velocity component errors E Nare σ2N =E ∆v2 = σ2E =E ∆v2 = 0.052 m2 /s2 v N v E (4.48)which gives heading angle variance v2 σ2N + v2 σ2E E v N v 0.052 σ2 =E ∆ψ2 = ψ = 2 rad 2 (4.49) v4 v 0.05i.e. σψ = v rad. —page 37—
  • 46. CHAPTER 5 Kalman Filter PerformanceThe different filters are evaluated using data from a test with the real autonomousvehicle. The test took place at a parking lot in Kista, Stockholm. The area overwhich the vehicle moved during the test is about 30 meters long and 15 meter wide.The surface on the parking lot was slightly wet asphalt.The purpose of the the tests made here are to evaluate the filterst’ ability to es-timate its parameters. Therefore a reference path is not derived and used. Insteadthere are two runs of every filter. One where the GPS data is available to the filterduring the whole path and another where the GPS data is removed after about threequarters of the path. The two filter results are then compared to each other andthe position and heading drift between them are derived. For further studies of thefilters ability to estimate a real path, a reference path has to be derived and newfilter studies made.Using the three sensors on the autonomous vehicle; the GPS, the odometry andthe gyro, the three sets of data in figure 5.1, 5.2 and 5.3 are derived. The path con-sisted of four laps, first two following the edges of the 30 meters times 15 metersparking lot; the first lap is marked blue on the GPS figure 5.1, the second green.The following two laps were made in the figure of an eight driving 15 meters eitherfollowing the edges of the parking lot or crossing it right in the middle, these pathsare marked red and at last black. The path is easier to study in the case of the gyroor odometer data in figure 5.2 and 5.3. Some deviations from the exact edges of the30 times 15 meter square were made which can be seen on the GPS plot, mainlyon the first lap. All GPS samples used in the filtering is marked with a red dot infigure 5.1.The noisy part of the GPS path in the lower left corner of the plot is the GPS re-ceiver filter starting to converge after the GPS is powered on. There is also a lotof noise at the end of the path. These parts of the data has been removed from thedata set during filter run.Figure 5.2 shows the odometer path derived from the test run. The odometer drift —page 38—
  • 47. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation Data from GPS reciever 15 10 5 North [m] 0 First lap Second lap Third lap Fourth lap −5 First datapoint used Last datapoint used −10 0 5 10 15 20 25 East [m] Figure 5.1: The GPS data derived from the test run in Kista. Odometer path 10 5 0 North [m] −5 −10 −15 Path Starting point −20 −5 0 5 10 15 20 25 30 35 East [m] Figure 5.2: The estimated vehicle path generated by odometer data.can be clearly seen with the odometer path drifting to the south-east.Since the gyro only provide the heading angular rate of the vehicle and not the ve-locity, the odometer velocity is used together with the gyro vehicle angular rate tovisualize the path calculated by the gyro. It can be seen that the gyro angular driftclearly differs from the odometer angular drift. The odometer drift consequently inone direction while the gyro has a more random drift in both directions.The following sections evaluate the different filters examined in this master thesis.The innovation plotted for each of the estimated state parameters is the difference ˆ ˜between predicted observation h(x) and the actual measurement y. This showshow much the model differs from the measurements made. If all parameters of thereal system are estimated correctly the innovation residual plot should have whitenoise characteristics. As will be seen in all of the innovation plots this is not thechase, especially not for the position. This is at least partially due to the Kalman —page 39—
  • 48. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation Gyro path 18 16 14 12 North [m] 10 8 6 4 2 Path Starting point 0 0 5 10 15 20 25 East [m] Figure 5.3: The gyro path derived from the test run.filter in the GPS receiver module that does not have a white noise error output andthat not all the parameters of the real system are correctly estimated. The reasonthese are not estimated is to keep the complexity of the system down.It proves that the system estimating its parameters and handling a GPS outagethe best is the filter using wheel radius error and odometry angular rate and gyroangular rate as measurements for heading angular velocity state 5.4.5.1 Odometry-GPS Estimating Wheel RadiusesThis filter is driven by the odometry, the GPS serves as a measurement for position,heading and velocity and the two rear wheel radiuses are estimated together withvehicle position and heading. The following plots show the estimations of param-eters the filter make.Figure 5.4 shows the wheel radiuses estimate as deviation from the precalibratedwheel radius of seven cm. Because of bad GPS data in the beginning of the testrun the first 80 seconds are run without the GPS measurement, thus no update ofthe radius is done. The radiuses wander away quite quickly at first before the ve-hicle start to have heading changes. As indicated by the linear observability thesystem is not observable unless both wheels are moving and the heading is chang-ing continually. It takes the filter quite long time to converge but a clear trend ofconvergence can be seen. First after about 150 seconds the filter has reached areasonable value of the radius. The condition for observability for this filter is thatboth vehicle wheels must turn and the heading must change continually. Since therun starts with an almost straight forward drive, the filter fails to estimate the wheelradius at first. After the first turns the radius start to converge. —page 40—
  • 49. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation −3 Wheel radius deviation x 10 3 Right wheel estimate Left wheel estimate 1−σ 3−σ 2 1 Wheel Radius Deviation [m] 0 −1 −2 −3 0 50 100 150 200 Time [s] Figure 5.4: Wheel radius estimate plot with the nominal value removed. Innovation, position 4 3 2 Innovation Residual [m] 1 0 −1 −2 Innovation, Position East Innovation, Position North −3 1−σ −4 80 100 120 140 160 180 200 Time [s]Figure 5.5: This plot shows the innovation of the position of the Kalman filtertogether with their 1-sigma standard deviation.The innovation residual of the position is plotted in figure 5.5 together with its 1−σstandard deviation. It might seem that the innovation standard deviation is set a bithigh. This is due to good satellite conditions during the test run. A high positionmeasurement noise is needed for the occasion of GPS drops where the measuredposition can make big jumps or if the satellite constellation is worse, thus givingless exact position information. The measurement noise is set using data from theGPS receiver data sheet [9]. As mentioned in subsection 2.1.2 the position infor-mation from the GPS receiver does not have a white noise error but is modeled assuch. This is seen in plot 5.5 as the innovation residual does not have white noisecharacteristics. The velocity innovation in plot 5.7 is more white noise like. Thevelocity is also measured by the GPS, but using the doppler shift. The doppler shifterror from the GPS receiver is more correctly modeled as a white noise thus giv- —page 41—
  • 50. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation Innovation, heading 0.2 0.1 Innivation Residual [rad] 0 −0.1 −0.2 Innovation, Heading 1−σ −0.3 3−σ 80 100 120 140 160 180 200 Time [s]Figure 5.6: This plot shows the innovation of heading of the the Kalman filtertogether with their 1-sigma standard deviation. Innovation, speed 0.6 0.4 0.2 Innovation Residual [v] 0 −0.2 −0.4 Innovation, Speed 1−σ 3−σ −0.6 80 100 120 140 160 180 200 Time [s]Figure 5.7: This plot shows the innovation of speed of the the Kalman filter togetherwith their 1-sigma standard deviation.ing the residual a more white noise characteristic. Around time 80 the residual forthe velocity has a few spikes. These are related to the jumps in wheel radius seenaround time 80 in figure 5.4. The innovation residual of the heading in plot 5.6also have a non white noise residual which indicate nonmodeled dynamics. Thesenon modeled dynamics could be related to wheel slips. Since the GPS headingmeasurement is calculated using the GPS velocity vectors the residual should havewhite noise characteristics. The non white characteristics can instead be relatedto the nonmodeled rear wheel axis width causing systematic errors in position ad-vance.The estimated path in figure 5.8 is very uneven, caused by the wrongly estimatedwheel radius being corrected by the GPS position and heading data. —page 42—
  • 51. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation Estimated Vehicle Path 15 10 North [m] 5 0 Estimated Path Starting Point −5 0 5 10 15 20 25 East [m]Figure 5.8: This is the estimated path when the GPS has been active during thewhole path. Estimated Vehicle Path 15 10 North [m] 5 0 Estimated Path Starting Point −5 0 5 10 15 20 25 30 East [m]Figure 5.9: Figure of the filter estimated path when GPS data is unavailable duringthe last lap.The error the filter does following the GPS removal in figure 5.9 seem to be mostlydue to an heading error at the start of the dead reckon. This since most of theheading error seem happen during the turn the vehicle is in when the GPS data isremoved. The turns following the first turn all seem to be close to 90 degrees asthey should. The data set just isn’t long enough to be able to make a reliable esti-mate of the wheel radius before the GPS is removed thus leading to poor headingestimation.In figure 5.10 the dead reckoning error grows quite fast after the GPS outage attime 180, this is because of the initial heading error at the start of the dead reckon-ing. The heading increases fast directly after the GPS is turned off. After a turn attime 210 the error starts to decrease a little again, this due to the turn made at thistime is a little too big lowering the heading error again. —page 43—
  • 52. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation 4 Integrated position error x 10 2.5 2 Integrated Position Error 1.5 1 0.5 0 0 50 100 150 200 250 300 350 Time [s] Figure 5.10: This plot shows the integrated distance error that the filter is doing. Heading Estimate Error 1 0.9 0.8 0.7 Heading Estimate Error [rad] 0.6 0.5 0.4 0.3 0.2 0.1 0 0 50 100 150 200 250 300 350 Time [s] Figure 5.11: Heading error plot.In figure 5.11 the initial heading error just after the GPS outage which causes theposition to drift is seen.5.1.1 Filter ConclusionsThe plots above show that this filter does not have the right combination of param-eters estimated or measurements done for a data set this short. The system driftsaway fast after a GPS outage. Perhaps a longer set of data would be able to estimatethe required parameters in a better more stabile way. The largest problem seems tobe the estimation of system heading. —page 44—
  • 53. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation5.2 Odometry-GPS Estimating Wheel Radiuses and Rear Wheel DistanceThis filter is driven by the odometry only, the GPS serves as a measurement forposition, velocity and the two rear wheel radiuses and the rear wheel axis width areestimated. The following plots show the parameter estimations the system make. x 10 −3 Wheel radius deviation 4 Right wheel estimate Left wheel estimate 1−σ 3 3−σ 2 Wheel Radius Deviation [m] 1 0 −1 −2 −3 −4 0 50 100 150 200 250 300 350 Time [s]Figure 5.12: Wheel radius estimate plot with its one and three sigma standarddeviation.The plot above shows the wheel radius estimate as deviation from the initial valueof seven cm. Because of bad GPS data in the beginning of the test run the first80 seconds are run without the GPS measurement thus no update of the radius isdone. The radius of this filter is converging faster than the filter estimating only thewheel radius 5.1. Rear wheel axis deviation 0.2 Base Width Estimate 1−σ 3−σ 0.15 0.1 Rear Wheel Axis Width Deviation [m] 0.05 0 −0.05 −0.1 −0.15 −0.2 0 50 100 150 200 250 300 350 Time [s]Figure 5.13: Base width estimate plot with one and three sigma standard deviation.As seen in figure 5.13, it is the base width that is affected by the poor observabilityof the system during the first straight driving and drift off over 0.15 meters. In filter —page 45—
  • 54. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation5.1 this error is affecting the wheel radius. Innovation, position 5 4 3 2 Innovation Residual [m] 1 0 −1 −2 Innovation, Position East Innovation, Position North −3 1−σ −4 −5 60 80 100 120 140 160 180 200 220 Time [s]Figure 5.14: Position innovation plot with one and three sigma standard deviation. Innovation, heading 0.3 Innovation, Heading 1−σ 0.25 3−σ 0.2 0.15 Innovation Residual [rad] 0.1 0.05 0 −0.05 −0.1 −0.15 −0.2 60 80 100 120 140 160 180 200 220 Time [s]Figure 5.15: Heading innovation plot with one and three sigma standard deviation.The innovation residual of the position is plotted in figure 5.14 together with its 1 −σ standard deviation. It might seem that the noise level is set a bit high. This is dueto good satellite conditions during the test run. A high position measurement noiseis needed for the occasion of GPS drops where the measured position can make bigjumps or if the satellite constellation is worse thus delivering worse position data.The measurement noise is set using data from the GPS receiver data sheet [9]. Asmentioned in subsection 2.1.2 the position information from the GPS receiver doesnot have a white noise error but is modeled as such. This is seen in plot 5.14 as theresidual does not have white noise characteristics. The innovation of the heading —page 46—
  • 55. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation Innovation, speed 0.8 Innovation, Speed 1−σ 0.6 3−σ 0.4 Innovation Residual [v] 0.2 0 −0.2 −0.4 −0.6 −0.8 60 80 100 120 140 160 180 200 220 Time [s]Figure 5.16: Velocity innovation plot with one and three sigma standard deviation.in plot 5.6 does also seem to have a non white noise residual which indicate nonmodeled errors. The velocity innovation in plot 5.16 is more white noise like. Thevelocity is also measured by the GPS, but using the doppler shift. The doppler shifterror from the GPS receiver is more correctly modeled as a white noise thus givingthe residual a more white noise like characteristic. Around time 80 the residual forthe velocity has a few spikes. These are related to the jumps in mainly rear wheelaxis width but also the wheel radius seen around time 80 seen in figure 5.13 and5.12. Estimated Vehicle Path 15 10 North [m] 5 0 Estimated Path Starting Point −5 0 5 10 15 20 25 East [m]Figure 5.17: The plot shows filtered position estimation where the GPS has beenavailable during the whole estimation time.The estimated path of this filter is very similar to that of system 5.1 and does nothave much more ability to estimate the filter parameters better for this amount ofdata.The dread reckon after the GPS outage seem very similar to the filter estimatingonly the wheel radiuses 5.1 apart from the initial heading error being a little smallerhere. By the time the GPS is turned off the base width estimation have reached a —page 47—
  • 56. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation Estimated Vehicle Path 15 10 North [m] 5 0 Estimated Path Starting Point 0 5 10 15 20 25 East [m] Figure 5.18: Estimated path where the GPS was turned off during the last lap.value more close to the real and thus the first turn of the dead reckon is estimatedbetter. A longer data set would probably give an better estimation of system pa-rameters and thus give better results. Integrated Position Error 14000 12000 10000 Integration Position Error 8000 6000 4000 2000 0 0 50 100 150 200 250 300 350 Time [s]Figure 5.19: Position difference between filter where GPS is used for the wholepath and where it is turned off during the last lap.The position error seem to be slightly smaller than for the filter estimating onlywheel radiuses 5.1, this is due to at the start of the dead reckon the heading is esti-mated closer to the real value.In figure 5.20 the heading estimation difference is a lot smaller at the start of thedead reckon but still grows quite fast during the left turns at the start of this part ofthe path. This heading drift is compensated then the vehicle starts to turn right andthen grows again when the vehicle turns left again at the end of the path. —page 48—
  • 57. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation Heading Estimate Error 0.7 0.6 0.5 Heading Estimate Error [m] 0.4 0.3 0.2 0.1 0 0 50 100 150 200 250 300 350 Time [s]Figure 5.20: Heading estimation difference between the path using GPS for thewhole path and the path where the GPS was turned off during the last lap.5.2.1 Filter ConclusionsThis filter preform slightly better than filter described in section 5.1. This filterwould also, as filter 5.1, gain from a longer data set to estimate its parameters. Per-haps with a larger data set, the two odometer filters performances will distinguishthemselves more from each other.5.3 Odometry-Gyro-GPS Estimating Gyro Bias and Scale- factorFor this filter the velocity is driven by the odometry, and the heading by the gyro.The GPS serves as a measurement for position and heading and the gyro bias andgyro scale factor are estimated. The following plots show the parameter estima-tions the system make.The gyro bias does not seem to change at all. The observability analysis done insection 4.4 indicated that the system would not be observable and this is probablywhat is seen here.As for the bias in plot 5.21 the scale factor in plot 5.22 does not change much.The innovation of the position is plotted in figure 5.23 together with its 1 − σ stan-dard deviation. It might seem that the noise level is set a bit high. This is due togood satellite conditions during the test run. A high position measurement noiseis needed for the occasion of GPS satellite drops where the measured position canmake big jumps or if the satellite constellation is worse. The measurement noiseis set using data from the GPS receiver data sheet [9]. As mentioned in subsection2.1.2 the position information from the GPS receiver does not have a white noiseerror but is modeled as such. This is seen in plot 5.23 as the residual does not have —page 49—
  • 58. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation x 10 −3 Bias Estimate −1.3 Bias Estimate 1−σ 3−σ −1.4 Bias Estimate [rad/s] −1.5 −1.6 −1.7 0 50 100 150 200 250 300 Time [s] Figure 5.21: Estimation of the gyro bias. Scale Factor Estimate 1−σ 1.1 3−σ 1.05 1 Scale Factor 0.95 0.9 0.85 0 50 100 150 200 250 Time [s] Figure 5.22: Scale factor estimation.white noise characteristics. The innovation of the heading in plot 5.6 has whitenoise characteristics. This change compared to when the heading is driven withthe odometry shows that it is not the GPS causing the heading residual non whitenoise characteristics but rather the odometry. The heading residual has some largespikes around time 90 corresponding to the big jumps in position seen in plot 5.25at about 15 meters east, 10 meter north. This is due to a bad GPS sample and iscorrected the following GPS sample.This filter seem to be a little more sensitive to bad GPS data than filters 4.2 and 4.3.The gyro seem to be able to estimate the path quite well. But note that the biasand scale factor are almost constant during estimation. An interesting comparisonis between the path after the GPS is removed in figure 5.26 and how the path driftfrom the same point forward in the gyro data 5.3 alone. They are not unexpectedlyvery similar since the scale factor and bias did not change during the filter estima-tion. —page 50—
  • 59. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation Innovation, position 5 4 Innovation, Position East Innovation, Position North 3 1−σ 2 Innovation Residual [m] 1 0 −1 −2 −3 −4 −5 60 80 100 120 140 160 180 200 Time [m] Figure 5.23: The Kalman filter position innovation. Innovation, heading 0.25 Innovation, Heading 1−σ 0.2 3−σ 0.15 0.1 Innovation Residual [rad] 0.05 0 −0.05 −0.1 −0.15 −0.2 −0.25 60 80 100 120 140 160 180 200 Time [s] Figure 5.24: The Kalman filter heading innovation.What is actually seen in figure 5.27 and 5.28 are the drift with hand trimmed val-ues since they did not change during the filter estimation. The filters ability to deadreckon with just hand trimmed values over preforms the odometry filters. This in-dicates that the gyro could be a good complement to odometry if it is correctly used.5.3.1 Filter ConclusionsThe linear filter observability analysis done for this filter structure in section 4.4indicated that the system is not observable. This seems to be true for the nonlinearsystem too since the gyro scale factor and bias does not change notably. This filteris not working correctly and the only information gained from the estimations isthat the gyro alone should correctly used be able to help the odometry in the esti-mation of heading quite well. —page 51—
  • 60. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation Estimated Vehicle Path 15 10 North [m] 5 0 Estimated Path Starting Point −5 0 5 10 15 20 25 30 East [m] Figure 5.25: Filter estimated path with GPS data during the whole test time. Estimated Vehicle Path 15 10 Nort [m] 5 0 Estimated Path Starting Point −5 0 5 10 15 20 25 30 East [m] Figure 5.26: Filter estimated path with GPS data removed during the last lap.5.4 Odometry-Gyro-GPS Estimating Wheel Radiuses Er- ror and Gyro BiasThis filter is run using odometry for velocity and a heading angular velocity es-timate for heading. The Gyro and odometry heading are used as a measurementdriving this heading angular velocity. The GPS is used for position, velocity andheading measurements.The plot above shows the wheel radius estimate as deviation from the initial valueof about seven cm. Because of bad GPS data in the beginning of the test run thefirst 80 seconds are run without the GPS measurement thus no update of the radiusis done due to the GPS. For this filter unlike the other three, the standard deviationstarts to decrease already at time 65. This is when the first gyro data starts to befed to the filter. Then at time 80 when the GPS data becomes reliable and is used,the standard deviation decreases even more and the wheel radius also settles at a —page 52—
  • 61. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation Integrated Position Error 9000 8000 7000 6000 INtegrated Position Error 5000 4000 3000 2000 1000 0 0 50 100 150 200 250 300 Time [s] Figure 5.27: Position drift plot. Heading Estimate Error 0.4 0.3 0.2 Heading Estimate Error [rad] 0.1 0 −0.1 −0.2 −0.3 −0.4 −0.5 0 50 100 150 200 250 300 Time [s]Figure 5.28: Heading difference between estimated path where GPS has been usedduring the whole path and where it has been disregarded during the last lap.reasonable value very fast. The linear observability analysis done in section 4.5indicated observability under the condition of both wheels turning which is muchstronger than the other filters which also needed continuous heading changes.The bias estimate converges quite fast after the initial noise. To be noted thou isthat the gyro bias converges to a value very close to zero while it should be around0.03 rad/s. The bias is probably engulfed into another filter parameter such as thewheel radius.The innovation of the position is plotted in figure 5.31 together with its 1 − σ stan-dard deviation. It might seem that the GPS measurement noise is set a bit high. —page 53—
  • 62. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation x 10 −3 Wheel Radius Error Estimate 3 Right wheel estimate Left wheel estimate 1−σ 2 3−σ 1 Wheel Radius Deviation [m] 0 −1 −2 −3 0 50 100 150 200 250 300 350 Time [s]Figure 5.29: Wheel radius estimate plot with its one and three sigma standarddeviation. Bias Estimate 0.3 Bias Estimate 1−σ 0.2 3−σ 0.1 Bias Estimate [rad/s] 0 −0.1 −0.2 −0.3 0 50 100 150 200 250 300 Time [s] Figure 5.30: The bias estimate.This is due to good satellite conditions during the test run. A high position mea-surement noise is needed for the occasion of GPS drops where the measured posi-tion can make big jumps or if the satellite constellation is less advantageous. Themeasurement noise is set using data from the GPS receiver data sheet [9]. As men-tioned in subsection 2.1.2 the position information from the GPS receiver does nothave a white noise error but is modeled as such. This is seen in plot 5.31 as theinnovation residual does not have white noise characteristics. The residual inno-vation of the heading in plot 5.32 has white noise characteristics thus should bequite well modeled. The heading residual has some large spikes around time 190corresponding to the big jumps in position seen in figure 5.34 at about 14 meterseast, 8 meter north. This is due to a bad GPS sample and is corrected the next GPSsample.The path seems to be very smooth during most of the run. Since the system headingis driven by the heading angular velocity the filter is given one more noise param- —page 54—
  • 63. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation Innovation, position 5 4 3 2 Innovation Residual [m] 1 0 −1 −2 Innovation, Position East Innovation, Position North −3 1−σ −4 −5 60 80 100 120 140 160 180 200 220 Time [m]Figure 5.31: Position innovation plot with one and three sigma standard deviation. Innovation, heading 0.4 Innovation, Heading 1−σ 0.3 3−σ 0.2 Innovation Residual [rad] 0.1 0 −0.1 −0.2 −0.3 60 80 100 120 140 160 180 200 220 Time [s]Figure 5.32: Heading innovation plot with one and three sigma standard deviation.eter that can be tweaked. This noise parameter sets how fast the heading angularvelocity is allowed to change the heading. One reason for the smooth path might bethe smoothing being done using this tweaking parameter. The spikes in the resid-ual of the velocity innovation seen in figure around time 80 is corresponding to theradius corrections seen at the the same time in the radius estimation plot 5.29. Thisis also seen in the path estimation in figure 5.34 at 23 meters east, 10 meters northas the GPS corrects the position and heading.The filter seems to drift quite slowly as seen in 5.37. The integrated position erroris about 5 times slower than for the filter estimating only wheel radius 5.1.The mean of the heading error seem to be close to zero as seen in figure 5.37 and alot smaller than for the other filters. —page 55—
  • 64. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation Innovation, speed 0.8 Innovation, Speed 1−σ 3−σ 0.6 0.4 Innovation Residual [v] 0.2 0 −0.2 −0.4 −0.6 −0.8 60 80 100 120 140 160 180 200 220 Time [m] Figure 5.33: Speed innovation plot with one and three sigma standard deviation. Estimated Vehicle Path 15 10 North [m] 5 0 Estimated Path Starting Point −5 0 5 10 15 20 25 East [m] Figure 5.34: Path estimated using GPS during the whole test time.5.4.1 Filter ConclusionsUsing the gyro together with the odometer heading seems to smooth the path. Twoof the main reasons for this filter working better than the others might be that theheading is driven trough the heading angular velocity driven by measurements fromthe odometer and the gyro. Or that the error dynamics is slower than the systemdynamics. Using error dynamics for wheel radii, δRr and δRl instead of systemdynamics Rr and Rl might be what is causing the faster convergence.5.5 Filter ComparisonThere is a strong connection between the linear observation analysis done and thefilter ability to estimate its variables. The three filters that have linear observabil-ity, filter 5.1, 5.2 and 5.4, also estimates their states better. The gyro filter, 5.3, that —page 56—
  • 65. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation Estimated Vehicle Path 16 14 12 10 North [m] 8 6 4 2 0 Estimated Path −2 Starting Point −4 0 5 10 15 20 25 East [m]Figure 5.35: Path estimated with the GPS disregarded during the last lap of thepath. Integrated Position Error 4500 4000 3500 3000 Integrated Position Error 2500 2000 1500 1000 500 0 0 50 100 150 200 250 Time [s]Figure 5.36: The position difference between the filter run with GPS during thewhole path and the filter disregarding the GPS during the last lap.did not have linear observability doesn’t have the ability to estimate its parame-ters. Filter 5.4 observes its states faster then the other filters, this might be becauseof the error dynamics used in the filtering being slower than the system dynamicsused in the other filters. The result is thus that the last filter, described in section5.4, estimates its parameters in the most effective way. This is the system to startwith if an investigation for filters ability to estimate a traveled path. A summarizeis done in table 5.1. —page 57—
  • 66. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation Heading Estimate Error 0.2 0.15 0.1 Heading Estimate Error [rad] 0.05 0 −0.05 −0.1 −0.15 0 50 100 150 200 250 300 350 Time [s]Figure 5.37: The heading difference between the filter run with GPS during thewhole path and the filter disregarding the GPS during the last lap. Advantages Disadvantages Filter section 5.1 Filter does estimate its parame- The filter parameters are esti- ters. mated very slowly. Filter section 5.2 Filter does estimate its parame- The filter parameters are esti- ters. mated very slowly. Filter section 5.3 Filter preformes well even if Filter only works if initial val- GPS measurement is lost, but ues are chosen very close to real only if initial values are chosen value. correctly. Filter section 5.4 Filter estimates convergers fast. Slightly more advanced system Filter is stable even if GPS mea- model and measurements. surements are lost. Table 5.1: The advantages and disadvantages of the different filters. —page 58—
  • 67. CHAPTER 6 Conclusions and Future WorkHere it is discussed what is concluded from the work of this thesis and what couldbe done to increase the quality of the navigational system further.6.1 ConclusionsThe objective of this master thesis were to develop and evaluate a few differentnavigation filters for a wheeled platform fitted with a GPS receiver, a gyro andodometry. The odometry sensors were to be developed and implemented on thereal platform.The odometry sensors were developed and tested successfully using magnets anda magnetic field sensor. This was the solution that suited the dirty and dusty envi-ronment at the wheels best.Further a series of filters were developed and tested. First two filters used only theodometry for divining velocity and heading with GPS as measurement for position,heading and velocity, third filter used odometry for velocity, a gyro for heading anda GPS as measurement for position, heading. The fourth filter used the odometryfor velocity, gyro and odometry as measurements for heading and GPS for mea-suring position, heading and velocity. An observability analysis of the filters weredone and concluded that the first two filters were observable under the conditionsthat both wheels are moving. These filters did have some problems converging butdid so after some time. The third filter was concluded not to be observable underany conditions and this filter did have big problems converging. Only if the initialconditions was chosen perfectly did the filter dead reckon correctly but then fil-tering was unnecessary. The fourth filter had one condition for observability, bothwheels must be moving. Using system error dynamics instead of system dynamicsseems to have made the convergence a lot faster. This last filter was the filter thatestimated its parameters best, and also in the shortest amount of time. This is thefilter to start experiment with if an path estimating filter is to be examined. —page 59—
  • 68. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation6.2 Future WorkDuring the work with this master thesis there were a few things that was left asidebecause of limited time or implementation problems. These could be studied fur-ther and perhaps improve the filtering results.6.2.1 Reference PathTo derive how suited the filters are for estimating a real path new tests has to bemade. These tests should be done using the real platform following a well knownpath so that this path can be used as a reference during Matlab simulations. Thefilters ability to follow the real path should now be studied.6.2.2 Error Detection AlgorithmAn error detection algorithm using either the GPS quality data delivered by theGPS receiver or a CUSUM test should be able to increase the filters ability to es-timate the vehicle path giving it the ability to remove or in any other way handlebad sensor data.6.2.3 Longer Data SetSince the vehicle platform is small, unstable and the wheels are very wide com-pared to their distance from each other the data set needs to be long for the filter toconverge. The data set used in this thesis was to short to get a stabile convergence.At least double the data set would be good.6.2.4 C Code ImplementationThe best filter should be implemented in C so that it can be run on the autonomousvehicle for real time testing.6.2.5 Mathematically Driven SystemIf the whole system dynamics would be driven by mathematics described by thecar dynamics instead of a sensor driven system like now, extra information wouldbe gained from the system. Like for instance the heading cannot change if the ve-hicle velocity is zero.6.2.6 Stopping EstimationThe best filter 4.5 should be adopted so that the estimation of wheel radiuses andgyro bias are not estimated during lost GPS position. If the wheel radiuses andthe gyro bias are estimated with no GPS the radiuses and bias can drift together.This since the radius difference between the wheels and the gyro bias is correlated.Making one wheel larger than the other and shifting the bias one way can not be —page 60—
  • 69. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigationdetected. If the vehicle is standing still, there can be some earnings from starting toestimate the gyro bias, but the wheel radiuses have to be kept still since the wheelradius is not observable during standstill.6.2.7 More Advanced FilterAnother filter was started at, trying to estimate the vehicle movement in all threedimensions. This filter estimated 19 different states but it was derived that a tooadvanced model was extremely hard to use together with this small and unstableplatform. There were also great difficulties finding exactly what model to use forthe system. This model could be further developed and could perhaps give goodresults. —page 61—
  • 70. BIBLIOGRAPHY[1] Per Enge Pratap Misra. The Global Positioning System. Ganga Jamuna Pr, 2006. ISBN 0 9709544 0 9.[2] Peter Strömbäck. Centralized gps/ins integration for urban navigation, 2003. FOI-R- -0847- -SE.[3] Sérgio Cunha. On the integration of inertial and gps data with an odometer for land vehicle navigation, 2003. Faculty of Engineering, Porto University, Portugal.[4] Christopher R. Carlson. Error sources when land vehicle dead reckoning with differential wheelspeeds, 2003. Stanford University, Stanford California.[5] Mohinder S. Grewal and Angus P. Andrews. Kalman Filtering, Theory and Practice Using Matlab. Wiley Interscience, 2001. ISBN 0 471 39254 5.[6] Daniel D. Gajski. Digital Design. Prentice Hall, 1997. ISBN 0 13 301144 5.[7] Micro-isu bp3010, 2005. http://www.bec-nav.de.[8] Ramjee Prasad and M Ruggieri. Applied Satellite Navigation Using Gps, Galileo, And Augmentation Systems. ARTECH HOUSE PUBLISHERS, 2005. ISBN 1 580 53815 0.[9] Gps superstar ii, 2005. http://www.novatel.ca. —page 62—