Tesis de posicionamiento

Study of Gyro and Differential Wheelspeeds forLand Vehicle Navigation

Anders Engman

July 6, 2006

Abstract

This 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.

Sammanfattning

Denna 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.

LIST OF FIGURES

2.1 Different types of gyros. . . . . . . . . . . . . . . . . . . . . . . 42.2 Encoder pulse waves. . . . . . . . . . . . . . . . . . . . . . . . . 62.3 Parameter definitions. . . . . . . . . . . . . . . . . . . . . . . . . 7

3.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. . . . . . . . . . . . . . . . . . . . . . . 18

4.1 Parameter definitions. . . . . . . . . . . . . . . . . . . . . . . . . 22

5.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—

Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation

5.15 Heading innovation plot with one and three sigma standard deviation. 465.16 Velocity innovation plot with one and three sigma standard deviation. 475.17 The plot shows filtered position estimation where the GPS has been

available during the whole estimation time. . . . . . . . . . . . . 475.18 Estimated path where the GPS was turned off during the last lap. . 485.19 Position difference between filter where GPS is used for the whole

path and where it is turned off during the last lap. . . . . . . . . . 485.20 Heading estimation difference between the path using GPS for the

whole path and the path where the GPS was turned off during thelast lap. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49

5.21 Estimation of the gyro bias. . . . . . . . . . . . . . . . . . . . . . 505.22 Scale factor estimation. . . . . . . . . . . . . . . . . . . . . . . . 505.23 The Kalman filter position innovation. . . . . . . . . . . . . . . . 515.24 The Kalman filter heading innovation. . . . . . . . . . . . . . . . 515.25 Filter estimated path with GPS data during the whole test time. . . 525.26 Filter estimated path with GPS data removed during the last lap. . 525.27 Position drift plot. . . . . . . . . . . . . . . . . . . . . . . . . . . 535.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 standarddeviation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54

5.30 The bias estimate. . . . . . . . . . . . . . . . . . . . . . . . . . . 545.31 Position innovation plot with one and three sigma standard deviation. 555.32 Heading innovation plot with one and three sigma standard deviation. 555.33 Speed innovation plot with one and three sigma standard deviation. 565.34 Path estimated using GPS during the whole test time. . . . . . . . 565.35 Path estimated with the GPS disregarded during the last lap of the

path. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 575.36 The position difference between the filter run with GPS during the

whole path and the filter disregarding the GPS during the last lap. 575.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—

LIST OF TABLES

3.1 Karnaugh map for software comparison minimization. . . . . . . 19

5.1 The advantages and disadvantages of the different filters. . . . . . 58

—page iii—

CONTENTS

1 Introduction 11.1 Background . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11.2 Motivation for Positioning . . . . . . . . . . . . . . . . . . . . . 21.3 Problem Definition . . . . . . . . . . . . . . . . . . . . . . . . . 2

2 Sensors and Sensor Fusion 32.1 Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

2.1.1 Gyro . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32.1.2 GPS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42.1.3 Odometry . . . . . . . . . . . . . . . . . . . . . . . . . . 5

2.2 Sensor Fusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62.2.1 GPS-Gyro Integration . . . . . . . . . . . . . . . . . . . 72.2.2 GPS-Odometry Integration . . . . . . . . . . . . . . . . . 72.2.3 GPS-Gyro-Odometry Integration . . . . . . . . . . . . . . 8

2.3 Kalman Filtering . . . . . . . . . . . . . . . . . . . . . . . . . . 82.3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . 82.3.2 The System Model . . . . . . . . . . . . . . . . . . . . . 82.3.3 Observer . . . . . . . . . . . . . . . . . . . . . . . . . . 8

2.3.3.1 Kalman Gain K and Covariance P . . . . . . . 92.3.3.2 Kalman Filter Dynamics . . . . . . . . . . . . . 92.3.3.3 Filter Initialization . . . . . . . . . . . . . . . . 9

2.3.4 Difference Between Sensors . . . . . . . . . . . . . . . . 102.3.5 Linear and Nonlinear Functions and Measurements . . . . 10

2.3.5.1 Linear and Nonlinear Convergence . . . . . . . 102.3.6 Extended Kalman Filter Mathematics . . . . . . . . . . . 10

3 Hardware Implementation 123.1 The Autonomous Vehicle . . . . . . . . . . . . . . . . . . . . . . 123.2 Odometer Construction . . . . . . . . . . . . . . . . . . . . . . . 13

3.2.1 Different Encoders . . . . . . . . . . . . . . . . . . . . . 143.2.1.1 Optical Tachometer . . . . . . . . . . . . . . . 143.2.1.2 Magnetic Tachometer . . . . . . . . . . . . . . 153.2.1.3 From Tachometer to Encoder . . . . . . . . . . 15

—page iv—


3.3 The Encoder Solution . . . . . . . . . . . . . . . . . . . . . . . . 163.3.1 Encoder Mechanics . . . . . . . . . . . . . . . . . . . . . 163.3.2 Encoder Electronics . . . . . . . . . . . . . . . . . . . . 173.3.3 Micro Controller Software . . . . . . . . . . . . . . . . . 193.3.4 Sensor Error Analysis . . . . . . . . . . . . . . . . . . . 20

4 Kalman Filter Implementation 224.1 Filter and System Variable Definitions. . . . . . . . . . . . . . . . 244.2 Odometry-GPS Estimating Wheel Radii. . . . . . . . . . . . . . . 26

4.2.1 Observability . . . . . . . . . . . . . . . . . . . . . . . . 274.3 Odometry-GPS Estimating Wheel Radii and Rear Wheel Axis Width 27

4.3.1 Observability . . . . . . . . . . . . . . . . . . . . . . . . 294.4 Odometry-Gyro-GPS estimating Gyro Bias and Scale Factor . . . 29

4.4.1 Observability . . . . . . . . . . . . . . . . . . . . . . . . 314.5 Odometry-Gyro-GPS Estimating Wheel Radiuses Error and Gyro

Bias . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 314.5.1 Observability . . . . . . . . . . . . . . . . . . . . . . . . 33

4.6 System and Measurement Noises Q and R . . . . . . . . . . . . . 344.6.1 System Noise . . . . . . . . . . . . . . . . . . . . . . . . 34

4.6.1.1 Wheel Radius Noise . . . . . . . . . . . . . . . 344.6.1.2 Rear Wheel Axis Width Noise . . . . . . . . . 354.6.1.3 Odometer Position Noise . . . . . . . . . . . . 354.6.1.4 Odometer Angle Noise . . . . . . . . . . . . . 354.6.1.5 Heading Angular Velocity . . . . . . . . . . . . 354.6.1.6 Rate Gyro Noise . . . . . . . . . . . . . . . . . 36

4.6.2 Measurement Noise . . . . . . . . . . . . . . . . . . . . 364.6.2.1 GPS Position Noise . . . . . . . . . . . . . . . 364.6.2.2 GPS Velocity Noise . . . . . . . . . . . . . . . 364.6.2.3 GPS Heading Noise . . . . . . . . . . . . . . . 36

5 Kalman Filter Performance 385.1 Odometry-GPS Estimating Wheel Radiuses . . . . . . . . . . . . 40

5.1.1 Filter Conclusions . . . . . . . . . . . . . . . . . . . . . 445.2 Odometry-GPS Estimating Wheel Radiuses and Rear Wheel Dis-

tance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 455.2.1 Filter Conclusions . . . . . . . . . . . . . . . . . . . . . 49

5.3 Odometry-Gyro-GPS Estimating Gyro Bias and Scalefactor . . . 495.3.1 Filter Conclusions . . . . . . . . . . . . . . . . . . . . . 51

5.4 Odometry-Gyro-GPS Estimating Wheel Radiuses Error and GyroBias . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 525.4.1 Filter Conclusions . . . . . . . . . . . . . . . . . . . . . 56

5.5 Filter Comparison . . . . . . . . . . . . . . . . . . . . . . . . . . 56

6 Conclusions and Future Work 596.1 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 596.2 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60

6.2.1 Reference Path . . . . . . . . . . . . . . . . . . . . . . . 606.2.2 Error Detection Algorithm . . . . . . . . . . . . . . . . . 60

—page v—

6.2.3 Longer Data Set . . . . . . . . . . . . . . . . . . . . . . 606.2.4 C Code Implementation . . . . . . . . . . . . . . . . . . 606.2.5 Mathematically Driven System . . . . . . . . . . . . . . . 606.2.6 Stopping Estimation . . . . . . . . . . . . . . . . . . . . 606.2.7 More Advanced Filter . . . . . . . . . . . . . . . . . . . 61

Bibliography 62

—page vi—

CHAPTER 1

Introduction

This 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 Background

By 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—

lindhe

Note

Förklarar inte förkortningen INS.


1.2 Motivation for Positioning

The 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 Definition

The 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—

CHAPTER 2

Sensors and Sensor Fusion

2.1 Sensors

There 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 Gyro

The 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—


(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 realrotational 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 GPS

The 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

x = x+[

v1(t)v2(t)

], (2.2)

—page 4—


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−16W 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 Odometry

Odometry 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.

v =ωrRr +ωlRl

2, (2.3)

where 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 =ωrRr−ωlRl

B, (2.4)

where 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—


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.

v =ωrRr + ωlRl

2+

va(t)Rr + vb(t)Rl

2, (2.5)

r =ωrRr− ωlRl

B+

vc(t)Rr + vd(t)Rl

B, (2.6)

where 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 Fusion

The 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—


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 Integration

GPS 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 Integration

Similar 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—


2.2.3 GPS-Gyro-Odometry Integration

The 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 Filtering

2.3.1 Introduction

The 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 Model

The 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 Observer

The 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—

lindhe

Note

Tillstånden borde skrivas med fet stil.


measurements.

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 P

P 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−k+1 = FkPkFTk +Qk (2.7)

Kk = P−k HTk (HkP−k HT

k +Rk)−1 (2.8)

Where Hk is the linearization of the measurement matrix h(x).

2.3.3.2 Kalman Filter Dynamics

For 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 Initialization

For 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—


and the estimated state vector.

2.3.4 Difference Between Sensors

If 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 Measurements

In 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 Convergence

As 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 Mathematics

For 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(xk) and h(xk) and the ma-trices Qk and Rk, each time step. Where f(xk) is the system time propagationtransfer function, h(xk) is the measurement function, Qk the system noise vari-ation 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(xk)+wk (2.9)

zk = h(xk)+vk (2.10)

—page 10—


Where f(xk) is the transfer function propagating the system vector xk to xk+1,h(xk) transfers xk to the observable data zk and vk and wk is white noise originat-ing from the sensor or system model error dynamics. The estimated states in thestate 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 functionFk 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 covariancematrix must be chosen so that the initial values of the states are within distance toreal value described by the covariance. Calculate Qk = E[wkwT

k ], Rk = E[vkvTk ]

and P−k = E[(xk− x−k )(xk− x−k )T ] where k = 0 and xk the actual state.

1. Calculate the Kalman gain.

Kk = P−k HTk (HkP−k HT

k +Rk)−1 (2.11)

2. Calculate new estimate of state vector with measured values zk.

xk = x−k +Kk(yk−h(xk)−) (2.12)

3. Calculate covariance matrix.

Pk = (I−KkHk)P−k (I−KkHk)T +KkRkKTk (2.13)

4. Propagate state vector.

x−k+1 = f(xk) (2.14)

5. Project covariance matrix.

P−k+1 = FkPkFTk +Qk (2.15)

6. Calculate linearized Fk and Hk, k = k +1, restart at point 1.

—page 11—

CHAPTER 3

Hardware Implementation

3.1 The Autonomous Vehicle

This 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—


Figure 3.2: Block diagram of the autonomous vehicle system.

studied in this master thesis.

3.2 Odometer Construction

There 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—


One 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 Encoders

There 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 Tachometer

There 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—


light 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 Tachometer

If 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 Encoder

A 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—


channel). 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 Solution

There 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 Mechanics

Because 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—


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 Electronics

The 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—


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—


S1 = 0 S2 = 0 S1 = 0 S2 = 1 S1 = 1 S2 = 1 S1 = 1 S2 = 0P1 = 0 P2 = 0 X + X -P1 = 0 P2 = 1 - X + XP1 = 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 Software

The 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 6= 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 ofabout 19 Hz ( 20.000.000

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—


3.3.4 Sensor Error Analysis

There 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—


vehicle studied in this thesis.

—page 21—

CHAPTER 4

Kalman Filter Implementation

In 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

ddt

x = f(x, t)+g(x, t)w(t), (4.1)

y = h(x, t)+v(t), (4.2)

—page 22—


where 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.

v =ωrRr +ωlRl

2(4.3)

For 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—


observability matrix as described in equation 4.4.

HHF

...HFk−1

, (4.4)

where 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—


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 andvNGPS of the velocity components and equation 4.6.

Rr The estimated right wheel radius.δRr The right wheel radius deviation from hand measured value.Rr +δRr The real right wheel radius.Rl The estimated left wheel radius.δRl The left wheel radius deviation from hand measured value.Rl +δRl 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 ωrRr+ωl Rl

2 derived from the odometer andthe 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.

ψ = arctan2(vEGPS

vNGPS

) (4.5)

—page 25—


vGPS =√

v2EGPS

+ v2NGPS

(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.

ddt

ENψRr

Rl

=

− ωrRr+ωlRr2 sin(ψ)

ωrRr+ωlRr2 cos(ψ)ωrRr−ωlRr

B00

+

wEodometry

wNodometry

wψodometry

00

(4.7)

The 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.

y1 =

ENψ

vGPS

= h1(x1)+v(t) =

ENψ

ωrRr+ωlRl2

+

vEGPS

vNGPS

vψGPS

vvGPS

. (4.8)

The 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—


F1 =∂f1

∂x1=

0 0 −vcos ψ −ωr sin ψ2

−ωl sin ψ2

0 0 −vsin ψ ωr cos ψ2

ωl cos ψ2

0 0 0 ωrB − ωl

B0 0 0 0 00 0 0 0 0

, (4.9)

Further, the measurement function h1 is already linear and its Jacobian matrix is

H1 =∂h1

∂x= x1 =

1 0 0 0 00 1 0 0 00 0 1 0 00 0 0 ωr

2ωl2

. (4.10)

4.2.1 Observability

For 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.

[H1

H1F1

]=

1 0 0 0 00 1 0 0 00 0 1 0 00 0 0 ωr

2ωl2

0 0 −vcos ψ − ωr sin ψ2 − ωl sin ψ

20 0 −vsin ψ ωr cos ψ

2ωl cos ψ

20 0 0 ωr

B − ωlB

0 0 0 0 0

(4.11)

As 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 matrixrank. One observations done is that if ωr and/or ωlare 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 WheelAxis Width


—page 27—


In 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.

ddt

ENψRr

RlB

=

− ωrRr+ωlRl2 sin(ψ)

ωrRr+ωlRl2 cos(ψ)ωrRr−ωlRl

B000

+

wEodometry

wNodometry

wψodometry

000

(4.12)

The 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.

y2(x) =

ENψ

vGPS

= h2(x2)+v(t) =

ENψ

ωrRr+ωlRl2

+

vEGPS

vNGPS

vψGPS

vvGPS

(4.13)

The 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.

F2 =∂f2

∂x2=

0 0 −vcos ψ −ωr sin ψ2

−ωl sin ψ2 0

0 0 −vsin ψ ωr cos ψ2

ωl cos ψ2 0

0 0 0 ωrB

− ωlB

− ωrRr−ωl RlB2

0 0 0 0 0 00 0 0 0 0 00 0 0 0 0 0

(4.14)

Further, the measurement function h2 is linearized and its Jacobian matrix is

H2 =∂h2

∂x2=

1 0 0 0 0 00 1 0 0 0 00 0 1 0 0 00 0 0 ωr

2ωl2 0

(4.15)

—page 28—


4.3.1 Observability


[H2

H2F2

]=

1 0 0 0 0 00 1 0 0 0 00 0 1 0 0 00 0 0 ωr

2ωl2 0

0 0 −vcos ψ − ωr sin ψ2 − ωl sin ψ

2 00 0 −vsin ψ ωr cos ψ

2ωl cos ψ

2 00 0 0 ωr

B− ωl

B− ωrRr−ωl Rl

B2

0 0 0 0 0 0

(4.16)

As seen in equation 4.16 all the columns are independent, thus the matrix has fullrank. One observations that is done, is that if ωr and/or ωlare zero the matrix doesnot have full rank thus is not observable. If ωrRr = ωlRl , 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 ωrRr = ωlRl thesystem is not observable for this k. Otherwise the system is observable. ωr = 0and ωl = 0 corresponds to the vehicle having no velocity meaning that the twowheel radius cannot be estimated. If ωrRr = ωlRl 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 ScaleFactor


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) =r(t)−b

s+ v1(t) (4.17)

—page 29—


4.17 is a variable substitution from 4.18

r1(t) = s1r(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.

ddt

ENψbs

=

− ωrRr+ωlRl2 sin(ψ)

ωrRr+ωlRl2 cos(ψ)−rs−b

00

+

wEodometry

wNodometry

wψgyro

00

, (4.19)

ψ 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.

y3 =

EN

ψGPS

= h3(x3)+v(t) =

ENψ

+

vEGPS

vNGPS

vψGPS

(4.20)

The 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.

F3 =∂f3

∂x3= x3 =

0 0 −vcos ψ 0 00 0 −vsin ψ 0 00 0 0 −1 −rgyro

0 0 0 0 00 0 0 0 0

, (4.21)

Further, the measurement function h2 is already linear and its Jacobian matrix is

H3 =∂h3

∂x3=

1 0 0 0 00 1 0 0 00 0 1 0 0

. (4.22)

—page 30—


4.4.1 Observability

For the filter in this section there is no k large enough to get full rank. All Fk−13 for

k 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.

[H3

H3F3

]=

1 0 0 0 00 1 0 0 00 0 1 0 00 0 −vcos ψ 0 00 0 −vsin ψ 0 00 0 0 −1 −rgyro

0 0 0 vcos ψ vcos ψrgyro

0 0 0 vsin ψ vsin ψrgyro

(4.23)

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 Bias


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

rodometry(t) = r(t)+ωrδRr− ωlδRl

B+ vodometry(t), (4.25)

—page 31—


This gives the system as equation 4.26.

ddt

ENψr

δRr

δRlb

=

− ωr(Rr+δRr)+ωl(Rl+δRl)2 sinψ

− ωr(Rr+δRr)+ωl(Rl+δRl)2 cosψ

r0000

+

wEodometry

wNodometry

wψr

wr

000

(4.26)

The 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.

y4.1 =

ENψ

vGPS

= h4.1(x4)+vGPS(t) =

ENψ

ωr(Rr+δRr)+ωl(Rl+δRl)2

+

vEGPS

vNGPS

vψGPS

vvGPS

(4.27)

y4.2 =[

rgyro],h4.2 +vgyro(t) = [r(t)+b]+

[vrGyro

](4.28)

y4.3 =[

rodometry],h4.3 +vodometry(t) =

[r(t)+

ωrδRr− ωlδRl

B

]+

[vrOdometry

]

(4.29)

ψ = arctan2(vEGPS

vNGPS

) (4.30)

vGPS =√

v2EGPS

+ v2NGPS

(4.31)

—page 32—


The 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.

F4 =∂f4

∂x4=

0 0 −vcos ψ 0 − ωr sin ψ2 − ωl sin ψ

2 00 0 −vsin ψ 0 ωr cos ψ

2ωl cos ψ

2 00 0 0 1 0 0 00 0 0 0 0 0 00 0 0 0 0 0 00 0 0 0 0 0 00 0 0 0 0 0 0

, (4.32)

Further 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

H4.1 =∂h4.1

∂x4=

1 0 0 0 0 0 00 1 0 0 0 0 00 0 1 0 0 0 00 0 0 0 ωr

2ωl2 0

, (4.33)

H4.2 =∂h4.2

∂x4=

[0 0 0 1 0 0 1

], (4.34)

H4.3 =∂h4.3

∂x4=

[0 0 0 1 ωr

B − ωlB 0

]. (4.35)

4.5.1 Observability

Here H is as equation 4.36.

H4 =

H4.1H4.2H4.3

(4.36)


—page 33—


1 0 0 0 0 0 00 1 0 0 0 0 00 0 1 0 0 0 00 0 0 0 ωr

2ωl2 0

0 0 0 1 0 0 10 0 0 1 ωr

B − ωrB 0

0 0 −vcos ψ − ωr sin ψ2 −ωl sin ψ

2 0 00 0 −vsin ψ ωr cos ψ

2ωl cos ψ

2 0 00 0 0 1 0 0 00 0 0 0 0 0 00 0 0 0 0 0 0

(4.37)

If rows one through six and row nine are used, the matrix has full rank unless ωr

and ω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 R

A 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 Noise

4.6.1.1 Wheel Radius Noise

The wheel radii are modeled as random constants and thus the corresponding sys-tem noise is zero.

—page 34—


4.6.1.2 Rear Wheel Axis Width Noise

The rear wheel axis width is modeled as a random constant and thus the systemnoise is zero.

4.6.1.3 Odometer Position Noise

The 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,

wEodometry =vaRr + vbRl

2cos(ψ), (4.38)

taken 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

σ2Eodometry

=E[w2

Eodometry

]=

0.0820.0722 +0.0820.0722

4= 0.004m (4.39)

The same value for the north noise variance is used, i.e. E[w2

Eodometry

]=σ2

Eodometry=

σ2Nodometry

. Consequently, the standard deviation used for the noises in east and northdirection are,

σNodometry = σEodometry = 1.66×10−5m. (4.40)

4.6.1.4 Odometer Angle Noise

The 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

wψodometry =VcRr + vdRl

B, (4.41)

taken 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

σψodometry =

√0.0064∗0.0722 +0.0064∗0.0722

0.412 = 0.02rad (4.42)

4.6.1.5 Heading Angular Velocity

If 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—


samples, 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 Noise

The 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 Noise

The 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 fivemeter radius. The variance is calculated from CEP using the formula

(1.2CEP√

2

)2[8]

and is the same in both coordinates. Thus E[v2

NGPS

]= σ2

NGPS= E

[v2

EGPS

]= σ2

EGPS=(

1.2∗5√2

)2and the standard deviations are σNGPS = σEGPS = 4.24 m.

4.6.2.2 GPS Velocity Noise

The 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 Noise

Call the velocity components in E and N direction vE and vn, respectively, andsuppose that the GPS gives the corresponding measurements

—page 36—

lindhe

Note

Felläst av mig. I förra versionen skrev jag att det var bra att han angav en standardavvikelse. Såg inte att han skrivit tiopotensen som exp(...).


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

∆ψ=ψ−ψ = arctan−vE −∆vE

vN +∆vn− arctan

−vE

vN=

vE∆vN − vN∆vE

v2 (4.47)

to first order, where v2 = v2E +v2

N . The variances for the velocity component errorsare

σ2vN

=E[∆v2

N]= σ2

vE=E

[∆v2

E]= 0.052m2/s2 (4.48)

which gives heading angle variance

σ2ψ=E

[∆ψ2] =

v2Eσ2

vN+ v2

Nσ2vE

v4 =0.052

v2 rad2 (4.49)

i.e. σψ = 0.05v rad.

—page 37—

CHAPTER 5

Kalman Filter Performance

The 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—


0 5 10 15 20 25−10

−5

0

5

10

15

Data from GPS reciever

East [m]

Nor

th [m

]

First lapSecond lapThird lapFourth lapFirst datapoint usedLast datapoint used

Figure 5.1: The GPS data derived from the test run in Kista.

−5 0 5 10 15 20 25 30 35

−20

−15

−10

−5

0

5

10

Odometer path

East [m]

Nor

th [m

]

PathStarting point

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 differencebetween 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—


0 5 10 15 20 25

0

2

4

6

8

10

12

14

16

18

Gyro path

East [m]

Nor

th [m

]

PathStarting point

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 Radiuses

This 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—

lindhe

Note

Oklart formulerat stycke.


0 50 100 150 200

−3

−2

−1

0

1

2

3x 10

−3 Wheel radius deviation

Time [s]

Whe

el R

adiu

s D

evia

tion

[m]

Right wheel estimateLeft wheel estimate1−σ3−σ

Figure 5.4: Wheel radius estimate plot with the nominal value removed.

80 100 120 140 160 180 200

−4

−3

−2

−1

0

1

2

3

4

Innovation, position

Time [s]

Inno

vatio

n R

esid

ual [

m]

Innovation, Position EastInnovation, Position North1−σ

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—


80 100 120 140 160 180 200

−0.3

−0.2

−0.1

0

0.1

0.2

Innovation, heading

Time [s]

Inni

vatio

n R

esid

ual [

rad]

Innovation, Heading1−σ3−σ

Figure 5.6: This plot shows the innovation of heading of the the Kalman filtertogether with their 1-sigma standard deviation.

80 100 120 140 160 180 200

−0.6

−0.4

−0.2

0

0.2

0.4

0.6

Innovation, speed

Time [s]

Inno

vatio

n R

esid

ual [

v]

Innovation, Speed1−σ3−σ

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—


0 5 10 15 20 25

−5

0

5

10

15

Estimated Vehicle Path

East [m]

Nor

th [m

]

Estimated PathStarting Point

Figure 5.8: This is the estimated path when the GPS has been active during thewhole path.

0 5 10 15 20 25 30

−5

0

5

10

15


East [m]

Nor

th [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—


0 50 100 150 200 250 300 3500

0.5

1

1.5

2

2.5x 10

4

Time [s]

Inte

grat

ed P

ositi

on E

rror

Integrated position error

Figure 5.10: This plot shows the integrated distance error that the filter is doing.

0 50 100 150 200 250 300 3500

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

Time [s]

Hea

ding

Est

imat

e E

rror

[rad

]

Heading Estimate Error

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 Conclusions

The 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—


5.2 Odometry-GPS Estimating Wheel Radiuses and RearWheel Distance

This 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.

0 50 100 150 200 250 300 350−4

−3

−2

−1

0

1

2

3

4x 10

−3 Wheel radius deviation

Time [s]

Whe

el R

adiu

s D

evia

tion

[m]


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.

0 50 100 150 200 250 300 350−0.2

−0.15

−0.1

−0.05

0

0.05

0.1

0.15

0.2Rear wheel axis deviation

Time [s]

Rea

r W

heel

Axi

s W

idth

Dev

iatio

n [m

]

Base Width Estimate1−σ3−σ

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—


5.1 this error is affecting the wheel radius.

60 80 100 120 140 160 180 200 220−5

−4

−3

−2

−1

0

1

2

3

4

5Innovation, position

Time [s]

Inno

vatio

n R

esid

ual [

m]


Figure 5.14: Position innovation plot with one and three sigma standard deviation.

60 80 100 120 140 160 180 200 220−0.2

−0.15

−0.1

−0.05

0

0.05

0.1

0.15

0.2

0.25

0.3Innovation, heading

Time [s]

Inno

vatio

n R

esid

ual [

rad]


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—


60 80 100 120 140 160 180 200 220−0.8

−0.6

−0.4

−0.2

0

0.2

0.4

0.6

0.8Innovation, speed

Time [s]

Inno

vatio

n R

esid

ual [

v]


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.

0 5 10 15 20 25

−5

0

5

10

15


East [m]

Nor

th [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—


0 5 10 15 20 25

0

5

10

15


East [m]

Nor

th [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.

0 50 100 150 200 250 300 3500

2000

4000

6000

8000

10000

12000

14000

Time [s]

Inte

grat

ion

Pos

ition

Err

or

Integrated Position Error

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—


0 50 100 150 200 250 300 3500

0.1

0.2

0.3

0.4

0.5

0.6

0.7

Time [s]

Hea

ding

Est

imat

e E

rror

[m]

Heading Estimate Error

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.


This 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-factor

For 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—


0 50 100 150 200 250 300−1.7

−1.6

−1.5

−1.4

−1.3x 10

−3 Bias Estimate

Time [s]

Bia

s E

stim

ate

[rad

/s]

Bias Estimate1−σ3−σ

Figure 5.21: Estimation of the gyro bias.

0 50 100 150 200 250

0.85

0.9

0.95

1

1.05

1.1

Time [s]

Sca

le F

acto

r

Scale Factor Estimate1−σ3−σ

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—


60 80 100 120 140 160 180 200−5

−4

−3

−2

−1

0

1

2

3

4


Time [m]

Inno

vatio

n R

esid

ual [

m]


Figure 5.23: The Kalman filter position innovation.

60 80 100 120 140 160 180 200−0.25

−0.2

−0.15

−0.1

−0.05

0

0.05

0.1

0.15

0.2


Time [s]

Inno

vatio

n R

esid

ual [

rad]


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.


The 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—


0 5 10 15 20 25 30−5

0

5

10

15


East [m]

Nor

th [m

]


Figure 5.25: Filter estimated path with GPS data during the whole test time.

0 5 10 15 20 25 30

−5

0

5

10

15


East [m]

Nor

t [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 Bias

This 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—


0 50 100 150 200 250 3000

1000

2000

3000

4000

5000

6000

7000

8000

9000

Time [s]

INte

grat

ed P

ositi

on E

rror

Integrated Position Error

Figure 5.27: Position drift plot.

0 50 100 150 200 250 300−0.5

−0.4

−0.3

−0.2

−0.1

0

0.1

0.2

0.3

0.4Heading Estimate Error

Time [s]

Hea

ding

Est

imat

e E

rror

[rad

]

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—


0 50 100 150 200 250 300 350−3

−2

−1

0

1

2

3x 10

−3 Wheel Radius Error Estimate

Time [s]

Whe

el R

adiu

s D

evia

tion

[m]


Figure 5.29: Wheel radius estimate plot with its one and three sigma standarddeviation.

0 50 100 150 200 250 300

−0.3

−0.2

−0.1

0

0.1

0.2

0.3

Bias Estimate

Bia

s E

stim

ate

[rad

/s]

Bias Estimate1−σ3−σ

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—


60 80 100 120 140 160 180 200 220−5

−4

−3

−2

−1

0

1

2

3

4


Time [m]

Inno

vatio

n R

esid

ual [

m]


Figure 5.31: Position innovation plot with one and three sigma standard deviation.

60 80 100 120 140 160 180 200 220−0.3

−0.2

−0.1

0

0.1

0.2

0.3


Time [s]

Inno

vatio

n R

esid

ual [

rad]


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—


60 80 100 120 140 160 180 200 220−0.8

−0.6

−0.4

−0.2

0

0.2

0.4

0.6

0.8Innovation, speed

Time [m]

Inno

vatio

n R

esid

ual [

v]


Figure 5.33: Speed innovation plot with one and three sigma standard deviation.

0 5 10 15 20 25−5

0

5

10

15


East [m]

Nor

th [m

]


Figure 5.34: Path estimated using GPS during the whole test time.


Using 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 Comparison

There 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—


0 5 10 15 20 25

−4

−2

0

2

4

6

8

10

12

14

16


East [m]

Nor

th [m

]


Figure 5.35: Path estimated with the GPS disregarded during the last lap of thepath.

0 50 100 150 200 2500

500

1000

1500

2000

2500

3000

3500

4000

4500Integrated Position Error

Time [s]

Inte

grat

ed P

ositi

on E

rror

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—


0 50 100 150 200 250 300 350−0.15

−0.1

−0.05

0

0.05

0.1

0.15

0.2Heading Estimate Error

Hea

ding

Est

imat

e E

rror

[rad

]

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 DisadvantagesFilter section 5.1 Filter does estimate its parame-

ters.The filter parameters are esti-mated very slowly.

Filter section 5.2 Filter does estimate its parame-ters.

The filter parameters are esti-mated very slowly.

Filter section 5.3 Filter preformes well even ifGPS measurement is lost, butonly if initial values are chosencorrectly.

Filter only works if initial val-ues are chosen very close to realvalue.

Filter section 5.4 Filter estimates convergers fast.Filter is stable even if GPS mea-surements are lost.

Slightly more advanced systemmodel and measurements.

Table 5.1: The advantages and disadvantages of the different filters.

—page 58—

CHAPTER 6

Conclusions and Future Work

Here 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 Conclusions

The 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—


6.2 Future Work

During 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 Path

To 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 Algorithm

An 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 Set

Since 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 Implementation

The 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 System

If 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 Estimation

The 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—


detected. 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 Filter

Another 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—

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 odometerfor land vehicle navigation, 2003. Faculty of Engineering, Porto University,Portugal.

[4] Christopher R. Carlson. Error sources when land vehicle dead reckoning withdifferential wheelspeeds, 2003. Stanford University, Stanford California.

[5] Mohinder S. Grewal and Angus P. Andrews. Kalman Filtering, Theory andPractice 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—

Education

Tesis de posicionamiento