Tesis de Posicionanmiento por Odometria

1. Study of Gyro and Differential Wheelspeeds for
Land Vehicle Navigation
Anders Engman
July 6, 2006

2. 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 mean
that they together give an increased navigational performance. The report mainly
focuses on the filters ability to estimate the filter states that is being used to support
the navigation filters during GPS blockouts. The filters ability to estimate a path is
not the main focus.
The sensor data used in the report comes from a wheel based experiment platform
equipped 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 sensordata
frå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åndsvariabler
som används för att stötta navigeringen vid bortfall av GPSdata. Filtrets förmåga
att 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 att
driva systemekvationerna, i olika kombinationer. De fyra filtren som undersöks es-
timerar, förutom fordonets position och riktning, även hjulradier, hjulbasavstånd,
gyrobias, gyroskalfaktor, hjulradiefel och riktningens vinkelhastighet i olika kom-
binationer.

3. LIST OF FIGURES
2.1 Different types of gyros. . . . . . . . . . . . . . . . . . . . . . . 4
2.2 Encoder pulse waves. . . . . . . . . . . . . . . . . . . . . . . . . 6
2.3 Parameter definitions. . . . . . . . . . . . . . . . . . . . . . . . . 7
3.1 The vehicle shown without the top cover. . . . . . . . . . . . . . 12
3.2 Block diagram of the autonomous vehicle system. . . . . . . . . . 13
3.3 Reflective tachometer schematics. . . . . . . . . . . . . . . . . . 14
3.4 Opacity encoders. . . . . . . . . . . . . . . . . . . . . . . . . . . 15
3.5 Encoder wheel assembly. . . . . . . . . . . . . . . . . . . . . . . 17
3.6 Encoder wheel assembly. . . . . . . . . . . . . . . . . . . . . . . 18
4.1 Parameter definitions. . . . . . . . . . . . . . . . . . . . . . . . . 22
5.1 The GPS data derived from the test run in Kista. . . . . . . . . . . 39
5.2 The estimated vehicle path generated by odometer data. . . . . . . 39
5.3 The gyro path derived from the test run. . . . . . . . . . . . . . . 40
5.4 Wheel radius estimate plot with the nominal value removed. . . . 41
5.5 This plot shows the innovation of the position of the Kalman filter
together with their 1-sigma standard deviation. . . . . . . . . . . . 41
5.6 This plot shows the innovation of heading of the the Kalman filter
together with their 1-sigma standard deviation. . . . . . . . . . . . 42
5.7 This plot shows the innovation of speed of the the Kalman filter
together with their 1-sigma standard deviation. . . . . . . . . . . . 42
5.8 This is the estimated path when the GPS has been active during the
whole path. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43
5.9 Figure of the filter estimated path when GPS data is unavailable
during the last lap. . . . . . . . . . . . . . . . . . . . . . . . . . . 43
5.10 This plot shows the integrated distance error that the filter is doing. 44
5.11 Heading error plot. . . . . . . . . . . . . . . . . . . . . . . . . . 44
5.12 Wheel radius estimate plot with its one and three sigma standard
deviation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45
5.13 Base width estimate plot with one and three sigma standard deviation. 45
5.14 Position innovation plot with one and three sigma standard deviation. 46
—page i—

4. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation
5.15 Heading innovation plot with one and three sigma standard deviation. 46
5.16 Velocity innovation plot with one and three sigma standard deviation. 47
5.17 The plot shows filtered position estimation where the GPS has been
available during the whole estimation time. . . . . . . . . . . . . 47
5.18 Estimated path where the GPS was turned off during the last lap. . 48
5.19 Position difference between filter where GPS is used for the whole
path and where it is turned off during the last lap. . . . . . . . . . 48
5.20 Heading estimation difference between the path using GPS for the
whole path and the path where the GPS was turned off during the
last lap. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
5.21 Estimation of the gyro bias. . . . . . . . . . . . . . . . . . . . . . 50
5.22 Scale factor estimation. . . . . . . . . . . . . . . . . . . . . . . . 50
5.23 The Kalman filter position innovation. . . . . . . . . . . . . . . . 51
5.24 The Kalman filter heading innovation. . . . . . . . . . . . . . . . 51
5.25 Filter estimated path with GPS data during the whole test time. . . 52
5.26 Filter estimated path with GPS data removed during the last lap. . 52
5.27 Position drift plot. . . . . . . . . . . . . . . . . . . . . . . . . . . 53
5.28 Heading difference between estimated path where GPS has been
used during the whole path and where it has been disregarded dur-
ing the last lap. . . . . . . . . . . . . . . . . . . . . . . . . . . . 53
5.29 Wheel radius estimate plot with its one and three sigma standard
deviation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54
5.30 The bias estimate. . . . . . . . . . . . . . . . . . . . . . . . . . . 54
5.31 Position innovation plot with one and three sigma standard deviation. 55
5.32 Heading innovation plot with one and three sigma standard deviation. 55
5.33 Speed innovation plot with one and three sigma standard deviation. 56
5.34 Path estimated using GPS during the whole test time. . . . . . . . 56
5.35 Path estimated with the GPS disregarded during the last lap of the
path. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57
5.36 The position difference between the filter run with GPS during the
whole path and the filter disregarding the GPS during the last lap. 57
5.37 The heading difference between the filter run with GPS during the
whole path and the filter disregarding the GPS during the last lap. 58
—page ii—

5. LIST OF TABLES
3.1 Karnaugh map for software comparison minimization. . . . . . . 19
5.1 The advantages and disadvantages of the different filters. . . . . . 58
—page iii—

6. CONTENTS
1 Introduction 1
1.1 Background . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1
1.2 Motivation for Positioning . . . . . . . . . . . . . . . . . . . . . 2
1.3 Problem Definition . . . . . . . . . . . . . . . . . . . . . . . . . 2
2 Sensors and Sensor Fusion 3
2.1 Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
2.1.1 Gyro . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
2.1.2 GPS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
2.1.3 Odometry . . . . . . . . . . . . . . . . . . . . . . . . . . 5
2.2 Sensor Fusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
2.2.1 GPS-Gyro Integration . . . . . . . . . . . . . . . . . . . 7
2.2.2 GPS-Odometry Integration . . . . . . . . . . . . . . . . . 7
2.2.3 GPS-Gyro-Odometry Integration . . . . . . . . . . . . . . 8
2.3 Kalman Filtering . . . . . . . . . . . . . . . . . . . . . . . . . . 8
2.3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . 8
2.3.2 The System Model . . . . . . . . . . . . . . . . . . . . . 8
2.3.3 Observer . . . . . . . . . . . . . . . . . . . . . . . . . . 8
2.3.3.1 Kalman Gain K and Covariance P . . . . . . . 9
2.3.3.2 Kalman Filter Dynamics . . . . . . . . . . . . . 9
2.3.3.3 Filter Initialization . . . . . . . . . . . . . . . . 9
2.3.4 Difference Between Sensors . . . . . . . . . . . . . . . . 10
2.3.5 Linear and Nonlinear Functions and Measurements . . . . 10
2.3.5.1 Linear and Nonlinear Convergence . . . . . . . 10
2.3.6 Extended Kalman Filter Mathematics . . . . . . . . . . . 10
3 Hardware Implementation 12
3.1 The Autonomous Vehicle . . . . . . . . . . . . . . . . . . . . . . 12
3.2 Odometer Construction . . . . . . . . . . . . . . . . . . . . . . . 13
3.2.1 Different Encoders . . . . . . . . . . . . . . . . . . . . . 14
3.2.1.1 Optical Tachometer . . . . . . . . . . . . . . . 14
3.2.1.2 Magnetic Tachometer . . . . . . . . . . . . . . 15
3.2.1.3 From Tachometer to Encoder . . . . . . . . . . 15
—page iv—

7. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation
3.3 The Encoder Solution . . . . . . . . . . . . . . . . . . . . . . . . 16
3.3.1 Encoder Mechanics . . . . . . . . . . . . . . . . . . . . . 16
3.3.2 Encoder Electronics . . . . . . . . . . . . . . . . . . . . 17
3.3.3 Micro Controller Software . . . . . . . . . . . . . . . . . 19
3.3.4 Sensor Error Analysis . . . . . . . . . . . . . . . . . . . 20
4 Kalman Filter Implementation 22
4.1 Filter and System Variable Definitions. . . . . . . . . . . . . . . . 24
4.2 Odometry-GPS Estimating Wheel Radii. . . . . . . . . . . . . . . 26
4.2.1 Observability . . . . . . . . . . . . . . . . . . . . . . . . 27
4.3 Odometry-GPS Estimating Wheel Radii and Rear Wheel Axis Width 27
4.3.1 Observability . . . . . . . . . . . . . . . . . . . . . . . . 29
4.4 Odometry-Gyro-GPS estimating Gyro Bias and Scale Factor . . . 29
4.4.1 Observability . . . . . . . . . . . . . . . . . . . . . . . . 31
4.5 Odometry-Gyro-GPS Estimating Wheel Radiuses Error and Gyro
Bias . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31
4.5.1 Observability . . . . . . . . . . . . . . . . . . . . . . . . 33
4.6 System and Measurement Noises Q and R . . . . . . . . . . . . . 34
4.6.1 System Noise . . . . . . . . . . . . . . . . . . . . . . . . 34
4.6.1.1 Wheel Radius Noise . . . . . . . . . . . . . . . 34
4.6.1.2 Rear Wheel Axis Width Noise . . . . . . . . . 35
4.6.1.3 Odometer Position Noise . . . . . . . . . . . . 35
4.6.1.4 Odometer Angle Noise . . . . . . . . . . . . . 35
4.6.1.5 Heading Angular Velocity . . . . . . . . . . . . 35
4.6.1.6 Rate Gyro Noise . . . . . . . . . . . . . . . . . 36
4.6.2 Measurement Noise . . . . . . . . . . . . . . . . . . . . 36
4.6.2.1 GPS Position Noise . . . . . . . . . . . . . . . 36
4.6.2.2 GPS Velocity Noise . . . . . . . . . . . . . . . 36
4.6.2.3 GPS Heading Noise . . . . . . . . . . . . . . . 36
5 Kalman Filter Performance 38
5.1 Odometry-GPS Estimating Wheel Radiuses . . . . . . . . . . . . 40
5.1.1 Filter Conclusions . . . . . . . . . . . . . . . . . . . . . 44
5.2 Odometry-GPS Estimating Wheel Radiuses and Rear Wheel Dis-
tance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45
5.2.1 Filter Conclusions . . . . . . . . . . . . . . . . . . . . . 49
5.3 Odometry-Gyro-GPS Estimating Gyro Bias and Scalefactor . . . 49
5.3.1 Filter Conclusions . . . . . . . . . . . . . . . . . . . . . 51
5.4 Odometry-Gyro-GPS Estimating Wheel Radiuses Error and Gyro
Bias . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52
5.4.1 Filter Conclusions . . . . . . . . . . . . . . . . . . . . . 56
5.5 Filter Comparison . . . . . . . . . . . . . . . . . . . . . . . . . . 56
6 Conclusions and Future Work 59
6.1 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59
6.2 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60
6.2.1 Reference Path . . . . . . . . . . . . . . . . . . . . . . . 60
6.2.2 Error Detection Algorithm . . . . . . . . . . . . . . . . . 60
—page v—

8. 6.2.3 Longer Data Set . . . . . . . . . . . . . . . . . . . . . . 60
6.2.4 C Code Implementation . . . . . . . . . . . . . . . . . . 60
6.2.5 Mathematically Driven System . . . . . . . . . . . . . . . 60
6.2.6 Stopping Estimation . . . . . . . . . . . . . . . . . . . . 60
6.2.7 More Advanced Filter . . . . . . . . . . . . . . . . . . . 61
Bibliography 62
—page vi—

9. 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 Research
Agency. I wish to pay a special thanks to Peter Strömbäck and Bengt Boberg for
the endless help with the project, my tutor at FOI, Petter Ögren for helping me start
up at FOI, my tutor at KTH, Magnus Lindé for the extra help when I needed it, my
examiner 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 the
vehicle using a camera or looking directly at the vehicle, the person holding the
remote control device is navigating through the terrain. The person controlling the
vehicle 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 a
computer. The computer then identifies objects on the camera image and navigates
using these. A computer is not nearly as advanced as a human brain and position
using 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 to
give it a map of places where it can and can not move. For the autonomous vehicle
to be able to use this map its position on the map, more or less exact, needs to be
known. There are a number of different devices developed that can keep track of
position 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 and
by integrating those forces the position, speed and orientation can be calculated
from initial values. Using the GPS the instantaneous position of the vehicle can
be decided, and this position has a limited absolute error. Using an odometer the
number of revolutions the wheels turn is counted and thus the distance the vehicle
has traveled can be calculated. The calculations needed to keep track of the vehicle
are a lot lower using these sensors than for the camera imaging navigation.
—page 1—

10. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation
1.2 Motivation for Positioning
The need for the autonomous vehicle to know its position very exactly has more
than one reason. One is to keep itself on the passable parts of the map or just to
follow a number of preset waypoints. An autonomous vehicle most often has at
least one more mission then just to navigate. One might be to identify the position
of objects. For the autonomous vehicle to find a position of an object with any kind
of 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. Adding
a low certainty of the position of the vehicle itself to the low certainty of the sensor
identifying 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, recyclable
navigation system. These three sensors have three unique sets of properties and
using sensor fusion the complementary properties can be used to obtain reliable
navigation.
The sensors are placed on a wheeled platform and an onboard computer performs
data logging of the sensor data. The sensor fusion is to be implemented and tested
in matlab using the logged data. The vehicle is outfitted with a PC-104 computer
for all onboard computations, a camera, an gyro, a GPS receiver and W-LAN. The
odometer solution is to be developed during the project.
The first task is to derive a solution for the odometer, to produce the mechanics
for the odometer and to make a mathematical model for it. Then a GPS-odometry,
GPS-gyro and GPS-odometry-gyro fusion filter should be developed and evaluated.
—page 2—

11. CHAPTER 2
Sensors and Sensor Fusion
2.1 Sensors
There are a number of sensors that can be used for navigation. All of them have
their advantages and drawbacks. For this thesis GPS, odometry and a gyro were
chosen 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 limited
absolute error, and odometry because it cannot be jammed, does not drift while the
vehicle 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 friction
less axis so that the flywheel does not change rotational axis even if the casing
holding the gyroscope does. The angular difference between the flywheel and the
casing can then be measured. A good gyroscope is an advanced and expensive
piece of equipment. Recently, solid state gyroscopes using micro technology have
been 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 same
problem, they may be called gyroscopes.
The gyro, also called rate gyro, however is not perfect. The rate gyro in this thesis is
a "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 scale
factor multiplied with it, thus making the output either smaller or larger depending
on 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 shows
either positive or negative rotation. The measured gyro signal can thus be written
as
r(t) = sr(t) + b + v(t),
˜ (2.1)
—page 3—

12. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation
(a): Gyroscope (b): MEMS Gyro
Figure 2.1: Different types of gyros.
where r(t) is the signal from the rate gyro, s is the scale factor, r(t) is the real
˜
rotational velocity and b is the bias and v(t) is white measurement noise.
One can estimate the angular change from initial direction to current direction of
a rate gyro by integrating the signal. To do this the error parameters have to be
taken care of in one way or another, or the estimated direction will drift away fast
from the real one. A rate gyro such as the one used in this thesis can only measure
angular 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. The
receiver internally generates signals identical to the ones from the satellites and by
correlating these with the incoming signals a measurement of signal propagation
time is obtained. This time corresponds to different distances of different satellites
and the position of the receiver can be calculated using the known position of the
satellites.
The time (range) measurements are afflicted with several errors where receiver
clock error, satellite clock error, ionosphere and troposphere delays, satellite posi-
tion error and noise are some of the most prominent ones. These errors all affect
the errors on calculated position [1]. In this thesis the position obtained from the
receiver is used with a simplified error model where the position error is regarded
as white noise, according to
v1 (t)
˜
x = x+ , (2.2)
v2 (t)
—page 4—

13. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation
˜
where x is the position indicated from the GPS, x is the actual position and, v1 (t)
and v2 (t) are white noise.
The GPS is based on very weak signals (10−16 W at the receiver) [1] p.283. That
is 19 dB lower than the thermal noise floor and therefore the GPS is sensitive to
jamming and interference, intentional or unintentional. The receiver also requires
a 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 this
problem is collaborative navigation [2]. The position update rate of a modern GPS
receiver is typically a few Hz which can provide problems for a control system if
the 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 between
them so that when one of them has its transition from either low to high or high to
low, the other is just in between two transitions. In this way the square waves from
the tachometers have a phase shift, the first tachometer waveform being ahead if
the wheel is turning forwards and the second tachometer waveform being ahead if
the wheel is turning backwards as shown in figure 2.2. The wheel speeds measured
on a vehicle with two encoders, together with the diameter of the wheel give the
distance the vehicle has moved according to equation 2.3.
ωr Rr + ωl Rl
v= , (2.3)
2
where Rr and Rl are the right and left wheel radius, respectively, ωr and ωl are the
right and left wheel speeds, respectively, and v is the rear wheel axis center velocity
as defined in figure 2.3.
Since the outer wheels travel further than the inner ones, in a turn the direction of
the vehicle, using two encoders, can also be calculated, thus enabling the calcu-
lation of current position relative to the initial position. The equation for vehicle
heading using odometry is
ωr Rr − ωl Rl
r= , (2.4)
B
where r is the rear wheel axis center angular velocity and B is the rear wheel axis
width as defined in figure 2.3.
A wheel compresses somewhat during travel over uneven terrain, thus changing
the wheel radius, and the wheels might easily slip when moving on gravel at high
velocities or if the platform hits some object in its environment. The encoders also
have a discretization error. These errors are accumulative over distance and the
estimated position drifts significantly after a while of travel.
—page 5—

14. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation
Figure 2.2: Encoder pulse waves.
Detecting wheel slips from the odometry is very hard since there is no way telling
when or where they happen. Because of this, the error of the odometry sensor is
hard to describe. In this thesis the odometry error is simplified and seen as a white
noise on the wheel encoders. The measurement in equation 2.5 is used for vehicle
velocity and 2.6 for vehicle angular rate.
˜ ˜
ωr Rr + ωl Rl va (t)Rr + vb (t)Rl
v= + , (2.5)
2 2
˜ ˜
ωr Rr − ωl Rl vc (t)Rr + vd (t)Rl
r= + , (2.6)
B B
where va (t), vb (t), vc (t) and vd (t) is white noise with a large enough variance to
describe both the possible slip error and the quantization error. The positive sign
between the two terms in the noise part of the heading equation 2.6 is due to that
white noise is symmetric around zero.
2.2 Sensor Fusion
The rate gyro and odometer drift over time and distance, the GPS does not. If
the autonomous vehiclet’s only mission is to navigate, using only the GPS may in
some applications be sufficient. The path planning system within the vehicle just
has to navigate with a margin large enough to the edges of the navigational area
that the vehicle is guaranteed to be within that area.
There are, however, a number of reasons why a higher accuracy is needed. The
positioning of an autonomous vehicle is most often used for more than just getting
from point A to point B. For many real life tasks it has a mission other than just
—page 6—

15. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation
Figure 2.3: Parameter definitions.
navigation. One of them might be to find the position of an object by looking at it
with an onboard camera or a laser scanner from a distance. The camera and laser
scanner themselves have uncertainties. If one adds the uncertainty of the GPS to
the uncertainty of the platform sensors, the position of the object will not be exact
enough 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 to
have 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 GPS
system is also dependent on radio connections to the satellites. These connections
can be jammed either intentional or unintentional. The GPS alone will not be good
enough; 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 gyro
has a small high frequency error but a much larger low frequency error causing the
position to drift over time. Combining the two would lead to an more accurate and
robust navigation system. In this thesis this is accomplished by using an Extended
Kalman Filter (EKF).
2.2.2 GPS-Odometry Integration
Similar to the GPS-gyro integration in 2.2.1 filtering together GPS and odometry
has many times proven to give good results [3], [4]. Here, as with the GPS-gyro
integration, the GPS is not totally reliable, and odometry does drift during GPS
blackouts.
—page 7—

16. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation
2.2.3 GPS-Gyro-Odometry Integration
The rate gyro and odometry produce a drift when used alone. However, the rate
gyro and odometry drift is not exactly the same. During a GPS blackout the rate
gyro and odometry can be weighed together and thus decrease the system drift. The
gyro can then compensate the odometry when the wheels slip during hard turns or
when 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 filter
was initially developed by Swerling in 1958, and then enhanced by Kalman in
1960 and Kalman and Bucy in 1961. A wide variety of Kalman filters have been
developed from Kalman’s original formulation. In its original form the Kalman
filter works for discreet linear systems. Later the so called extended Kalman filter
(EKF) was developed which also handles nonlinear systems. In this thesis an EKF
is 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 linear
system. The issue is that the states cannot be measured directly but rather have to
be estimated though a series of measurements affected with white noise. There are
a number of filters doing this but the way the variation of the white noise of the
measurements 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 real
numbers x, each number describing one of the system states. At each discrete time
step 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 operator
or nonlinear function h(xk ) is then used to transfer the system measurements to the
system vector. These measurements are affected with measurement noise called
Rk . 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 be
observable 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 propagate
the 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 that
describes the state update. This stage can also be made multiple times between two
—page 8—

17. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation
measurements.
Measurements made by other sensors is then made to correct errors the prediction
stage 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 the
system since the measurement noise will be correlated with the system noise. The
weighting done between the predicted state and the measured state is calculated
using the noise of the measurement and the covariance matrix, P. The covariance
matrix is calculated for every prediction stage as shown in equation 2.7 and de-
scribes the system states estimate uncertainties. This weighting between the two
noises is done in an optimal way yielding a minimum variance estimate using the
two 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 these
equations has been derived is described in [5] p. 117-121.
P− = Fk Pk FT + Qk
k+1 k (2.7)
Kk = P− HT (Hk P− HT + Rk )−1
k k k k (2.8)
Where Hk is the linearization of the measurement matrix h(x).
2.3.3.2 Kalman Filter Dynamics
For the optimality of K and P to be calculated the noise parameters of R and Q also
has to be chosen correctly. Q describes as mentioned above the noise of the system
state prediction. If the system states propagation is driven by measurements the
noise of Q is taken from the error dynamics of the sensors. If the system states
propagation is driven by a mathematical model, the noise of error dynamics of the
model 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 studied
in [5]. It is usually hard to find an exact value of the noises in R and Q and it is
common 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 are
non modeled errors dynamics in the measurements or system model, the noise in R
and Q can be increased so that these errors also include the non modeled errors. If
these 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 covariance
has to be chosen large enough to include the difference between the actual state
—page 9—

18. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation
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 have
two types of Sensors. Sensors used to drive the system prediction and sensors used
as measurements to correct the prediction. Typically the sensors used to drive the
system prediction are sensors that have a high output rate and only low frequency
noise. Sensors used for measurements typically have a lower output frequency, a
higher 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 filter
calculations uses matrix inversions. If the system is not linear, it is not possible
to put the equations on matrix form. The prediction stage can still be done in non
linear 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 be
linearized and transposed for this purpose. Same system is applicable for the non
linear measurements. The difference between measured value and estimated value
can be calculated using the non linear equations, but calculations of the kalman
filter gain, equation 2.8, require matrix form and thus the measurement function
has to be linearized.
2.3.5.1 Linear and Nonlinear Convergence
As described in [5], chapter 4, convergence and optimality can be proven for linear
filters. 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 to
determine 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 and
works 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 to
the Kalman filter framework, specifying the functions f(ˆ k ) and h(ˆ k ) and the ma-
x x
trices Qk and Rk , each time step. Where f(ˆ k ) is the system time propagation
x
transfer function, h(ˆ k ) is the measurement function, Qk the system noise vari-
x
ation matrix and Rk the measurement noise variation matrix. Given the discreet
nonlinear system 2.9 and 2.10 the method for using a Kalman filter is as follows.
ˆ
xk+1 = f(ˆ k ) + wk
x (2.9)
zk = h(ˆ k ) + vk
x (2.10)
—page 10—

19. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation
ˆ ˆ
Where f(ˆ k ) is the transfer function propagating the system vector xk to xk+1 ,
x
ˆ
h(ˆ k ) transfers xk to the observable data zk and vk and wk is white noise originat-
x
ing from the sensor or system model error dynamics. The estimated states in the
ˆ
state vector xk covariance Pk is updated for all time steps and after all measure-
ments. For all time steps the linearized system time propagation transfer function
˜
Fk and the linearized measurement function Hk is calculated. yk is an external
measurement. The Kalman filtering is done as follows.
Choose initial value for the state vector x− . The initial value of the covariance
ˆ
matrix must be chosen so that the initial values of the states are within distance to
real value described by the covariance. Calculate Qk = E[wk wT ], Rk = E[vk vT ]
k k
and P− = E[(xk − x− )(xk − x− )T ] where k = 0 and xk the actual state.
k ˆk ˆk
1. Calculate the Kalman gain.
Kk = P− HT (Hk P− HT + Rk )−1
k k k k (2.11)
2. Calculate new estimate of state vector with measured values zk .
xk = x− + Kk (˜ k − h(ˆ k )− )
ˆ ˆk y x (2.12)
3. Calculate covariance matrix.
Pk = (I − Kk Hk )P− (I − Kk Hk )T + Kk Rk KT
k k (2.13)
4. Propagate state vector.
x− = f(ˆ k )
ˆ k+1 x (2.14)
5. Project covariance matrix.
P− = Fk Pk FT + Qk
k+1 k (2.15)
6. Calculate linearized Fk and Hk , k = k + 1, restart at point 1.
—page 11—

20. CHAPTER 3
Hardware 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 mm
wide.
Figure 3.1: The vehicle shown without the top cover.
The top cover is removed and replaced by an aluminum mounting plate. The
mounting plate is equipped with a PC/104 computer running Linux. The car is
also equipped with a servo control device, a camera, a GPS receiver and antenna,
an inertial navigation system and a wireless LAN, all connected to the PC/104.
Figure 3.2 is a schematic diagram over the components of the autonomous vehicle
—page 12—

21. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation
Figure 3.2: Block diagram of the autonomous vehicle system.
studied in this master thesis.
3.2 Odometer 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 encoder
sends a signal to a microcontroller or computer that keeps count of how far the
vehicle has traveled since the counting started. If encoders have been placed on
one left and one right wheel, the direction of the vehicle can also be calculated as
seen in subsection 2.1.3. One error of the measurements is due to slipping of the
wheels 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 tires
will compress and thus change their radius; this causes the calculated stretch of
the wheels to differ from the real stretch, which affects both distance traveled and
vehicle 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 in
a way that microslip can occur between the rim and the tire. This means that the
tires might slowly turn around on the rims when the vehicle is driving. Since the
two latter error sources are small compared to the slip caused by uneven terrain,
they may be disregard. The microslip appears evenly distributed over the distance
traveled and the distance error microslip will be handled by a filter estimating the
wheel radii.
—page 13—

22. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation
One way to handle the slip and microslip problems is to add one or more wheels
to the car, have them springloaded towards the ground at all time and then put the
encoder on these wheels. Since they are not driving there will be no micro slippage
and the wheels are a lot less likely to spin. The problem with this solution is that
the car mechanics will be more advanced and developing the system will take a lot
of time. In the case of the autonomous vehicle in this master thesis, it is easier to
use the standard wheels.
3.2.1 Different Encoders
There are a few different types of encoders. In this section their advantages and
disadvantages will be discussed. A tachometer is a primitive part of an encoder as
explained 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. The
reflex tachometer has the light source and the light sensor close to each other on
the same side of the tachometer disc. The tachometer disc is painted with many
small sections of black and white across the disc circumference, the black areas
reflecting less light than the white areas. The light sensitive sensor registers how
much 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 the
numer 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 the
light sensitive sensor on the other. The transparency tachometer disc has holes in
it, 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 higher
difference between light and dark areas may be achieved. The drawback is that the
—page 14—

23. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation
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 plastic
component in the figure to the left is the light source and the white transparent
component is the light sensitive sensor. Between them the tachometer disc can be
seen with its slots where the light is let trough.
The largest drawback for both of these tachometers is dirt. When dirt or dust gets
stuck on the black and white areas of the reflective wheel or in the holes of the
transparency encoder the readings will be corrupt. Since the thesist’ autonomous
vehicle 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 magnetic
north and south poles, a magnetic field sensor can be used to sense the change in
magnetic field instead of light intensity. One sensor that can sense magnetic field
is the Hall Effect sensor. The Hall Effect sensors can be placed at a distance from
the magnets and the magnetic field will not be disturbed by dirt or dust. Hall Effect
sensors 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 direction
of 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. For
some applications one more channel is needed. This channel is called I (for index
—page 15—

24. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation
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 turned
on, 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. This
correction is done by keeping track of how many ticks there are on a lap, and how
many has been counted since the last time the index was counted. If for instance
nine positive counts has been made since the last index, and there should be ten in
a lap, the electronics know that one tick was missed and this can be corrected by
adding one positive count.
3.3 The Encoder Solution
There are three parts that has to be done to make an encoder for the autonomous
vehicle 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 transform
the 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 and
at the same time handle the serial communication to the vehicle onboard computer.
Third, the software has to be efficient enough that the limited computational power
of 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 magnetic
strength 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. Neodymium
is very corrosive and protection from moist and dirt is needed.
The neodymium magnets are fixed to the rims using a system of thin lamellas. To
get high enough precision together with low weight the lamellas are made using
a water cut machine cutting the lamellas from 1.2mm thick plastic sheets. Water
cutting machines have about five hundreds of a millimeter of precision, which is
considered to be high enough. At both ends of the lamellas one more lamella is
placed to keep the magnets from moving coaxially.
Keeping the weight low is needed since the sensors are placed at the wheel of
the vehicle, and unspringed weight should be kept as low as possibly to reduce the
energy of that mass when moving over uneven terrain.
The lamellas are placed as far as possible towards the inner side of the wheels
—page 16—

25. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation
Figure 3.5: Encoder wheel assembly.
so that the sensors are easy to place on the other side of the rims. The magnets are
placed with every other magnet having its magnetic north pole facing outwards,
and every other inwards. The lamellas are fixed using screws to hold the lamellas
together with the magnets in their slots. The lamellas are glued to the ribs using a
two component glue.
3.3.2 Encoder Electronics
The purpose of the encoder electronics is to count the transitions of magnetic field
sensed by the Hall Effect sensors and send the number of transitions done during a
sample period to the vehicle onboard PC/104. The sensors are placed half a magnet
width from each other, so the pulse wave from the magnets were 90 degrees shifted
as 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 would
make the square waves a little different when the wheel is turning clockwise and
when it is turning counterclockwise, they were therefore dismissed. The two ana-
log sensors were Allegrot’s A3515 and A3518. A3515 has a maximum sensitivity
of 10 G and A3518 20 G. Both sensors were tested with the magnetic encoder disc
and it proved that neodymium magnets were strong enough to saturate both sensors
but A3518 had, because of its longer sensitivity range, a little smoother transition
between high pulse and low pulse. Therefore A3518 was chosen. The sensors were
fastened 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), so
that any positive magnetic field sensed generates a logic 1 (5 volt), and any neg-
—page 17—

26. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation
Figure 3.6: Encoder wheel assembly.
ative magnetic field sensed generates a logic 0 (0 volt). These logic level square
waves 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 the
counting a Atmel AVR micro controller is chosen. The Atmel series of 8-bit AVR
controllers 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 the
AVR micro controller using four interrupt driven inputs. Each time one of the
signals change state a certain code is run in the micro controller to decide which
channel changed and if the count to make is positive or negative.
When moving at slow velocities the sensors bounce a few times before settling
at the new state. The expression "bounce" means that the signal changes from one
state to the other a few times back and forth before settling at the new state. If
the amount of bounces is too high the micro controller will not be able to count
all the transitions and thereby fail to count correctly. To reduce bouncing a 2.2µF
capacitor (C3-C6 in figure 3.6) is added between the analog signal and ground to
smooth the Hall Effect sensor output. When this is done almost all of the bouncing
disappears, and for higher velocity there is no bouncing at all.
—page 18—

27. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation
S1 = 0 S2 = 0 S1 = 0 S2 = 1 S1 = 1 S2 = 1 S1 = 1 S2 = 0
P1 = 0 P2 = 0 X + X -
P1 = 0 P2 = 1 - X + X
P1 = 1 P2 = 1 X - X +
P1 = 1 P2 = 0 + X - X
Table 3.1: Karnaugh map for software comparison minimization.
3.3.3 Micro Controller Software
The purpose of the software is to keep track of the number of passing magnetic
field 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 states
that the input could have. Binary 0000, 0001, 0010, and so on. To know if one
should be added or subtracted, the prior state has to be remembered. From each
wheel there are only two prior states from which a transition to the present state
can be made. If the program is split up into two parts, one for each wheel sensor
pair, then each sensor has four states that each can be transitioned to from two prior
states. That means that eight comparisons have to be made for each sensor pair to
derive if it is an positive or negative transition, 16 totals for both wheels. Even thou
the 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 to
calculate a transition has to be as short as possible. Not all combinations of the two
bits of new sensor data and two bits of prior sample data is possible, they can only
make transitions from 1 to 2 to 3 to 4 to 1 again, or the opposite direction. A jump
from 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 transition
should lead to an increase of the number of transitions, - marks the decrease and X
marks an illegal change. Note that no change is seen as an illegal change since if
no 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. Given
the Karnaugh map rules the only comparison that has to be made is between S1 and
P2 . S1 = P2 produces an increase of transitions and S1 = P2 a decrease. The micro
controller now only has to do two times two comparisons instead of the earlier two
times eight comparisons, effectively now cutting the computation time in four.
The calculations of the transitions is set to be done during the fix time period of
20.000.000
about 19 Hz ( 1024∗256∗4 Hz). The numbers of calculated transitions are then sent
via the RS-232 serial link to the PC-104 and the two transition counters are reset
to zero and then start calculating again.
—page 19—

28. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation
3.3.4 Sensor Error Analysis
There are a few different errors of the odometer sensor. Quantization, nonlinearity
and missed tick are the most prominent.
Quantization error is the largest right before and right after a transition. If the
quantization 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 the
transition. For the wheel encoder this quantization is about 0.16 radians thus the
largest 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 this
quantization does cause problem is during slight turns. If one wheel should make
for 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 the
distance the vehicle traveled or the heading of the vehicle to differ from the real
value over time, but it does affect the estimated position. This since the position
depends 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 moves
during this error time the calculation for position change will be in slightly wrong
direction.
Linearity error is very small since the mounting for the magnets inside the rims
were water cut with a precision of about of about 2 micrometers. The error is thus
3 × 10−5 radians and can be neglected compared to quantization error. This error
does not increase by time.
If the wheel is spinning too fast, either the micro controller or the comparator
might not be able handle the frequency of transitions from the encoder. The wheel
radius is about 72 cm so the circumference of the wheel is about 0.45 meters. If the
vehicle 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’s
5.200 transitions for the micro controller to count using both encoders. Every tick
uses two if statements each being about 20 instructions for the micro controller
which is 208.000 instructions totally. The micro controller computes 20 million
instructions 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 taken
into account when setting the odometry noise. The nonlinearity is a lot smaller
than 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 the
counting of ticks from the encoders for any realistic velocity of the autonomous
—page 20—

29. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation
vehicle studied in this thesis.
—page 21—

30. 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 thesis
a flat earth representation is used providing navigation states expressed in east and
north 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 the
filters have been copied from [4], in which this definition is used.
The system equations for all the systems are on the form
d
x = f(x,t) + g(x,t)w(t), (4.1)
dt
y = h(x,t) + v(t), (4.2)
—page 22—

31. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation
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) is
the measurement function and v(t) is the measurement noise. In the sequel, these
equations 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 wheel
radii, 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 trouble
to find a good estimates for the wheel radii and the rear wheel axis width. When
GPS data is not present, resulting in position and heading drift.
In order to overcome this problem, a filter is tested where a heading gyro predicting
the vehicle heading is used. This filter is described in section 4.4. The odometer
heading information is disregarded in this system. This filter is designed to estimate
position, heading, gyro bias and gyro scale factor and uses the odometry velocity.
This system gives slightly better results than the GPS-odometry integration but it
still have quite a large drift. This drift, however, does not have the same features
as the odometer drift. The odometry has a steady drift towards one direction while
the gyro seems to have a more random drift back and forth.
A filter using the external measurements of the vehicle heading angular rate from
both the odometry and gyro is developed in subsection 4.5. The vehicle estimated
heading angular rate is there made to drive the system heading. For a measurement
function for the odometry to be determined, the wheel radius error instead of the
wheel radius it self has to be estimated. Thus an initial guess of the wheel radius
has to be made, from which the wheel radius errors are estimated. For this system
the 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 the
heading angular rate since the odometer and gyro measurements do not execute at
the same time. However, for a certain value of noise the system runs smoothly also
during GPS outages.
For all the filters, the vehicle ground velocity v is obtainable at all time steps using
equation 2.3, which is repeated as equation 4.3 for convenience.
ωr Rr + ωl Rl
v= (4.3)
2
For each filter the linear observability is analyzed. For the Kalman filter to be able
to 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 mathematics
for nonlinear observability is very complex this is not done in this thesis. Linear
observability does not prove nonlinear observability, but still gives a hint about the
observability of the system at least around the linearization point.
Linear observability is made by looking at the Jacobian matrices H and F of the
measurement function h and the system functions f , respectively, and construct the
—page 23—

32. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation
observability matrix as described in equation 4.4.
 
H
 HF 
 
 .
. , (4.4)
 . 
HF k−1
where k is large enough to get a matrix of full rank, if possible. If there is no k large
enough 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 are
only used in one or two filters. All variables are defined here.
—page 24—

33. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation
E The east position.
˜
E The east position measurements from the GPS.
N The north position.
˜
N The north position measurements from the GPS.
ψ The estimated vehicle heading angle from north to west
˜
ψ The GPS heading calculated using the GPS measurement vEGPS ˜
and vNGPS of the velocity components and equation 4.5. For veloc-
˜
˜
ities lower than 0.5 m/s the ψ is disregarded because of its inaccu-
racy.
vGPS
˜ the GPS velocity calculated from the GPS measurement vEGPS and
˜
vNGPS of the velocity components and equation 4.6.
˜
Rr The estimated right wheel radius.
δRrˆ The right wheel radius deviation from hand measured value.
˜ r + δRr
R ˆ The real right wheel radius.
Rl The estimated left wheel radius.
δRlˆ The left wheel radius deviation from hand measured value.
˜ l + δRl
R ˆ The real left wheel radius
˜
ωr The measurements of the right wheel angular rate using the en-
coders.
˜
ωl The measurements of the left wheel angular rate using the en-
coders.
B The rear wheel axis width.
˜ ˆ ˜ ˆ
v
˜ The measured velocity ωr Rr +ωl Rl derived from the odometer and
2
the wheel radii.
r The real vehicle angular rate.
r
˜ The gyro measured vehicle angular rate.
b The gyro bias.
s The gyro scale factor.
vgyro (t) The gyro angular rate noise as described in section 4.6.
vEGPS The GPS east position measurement noise as described in section
4.6.
vNGPS The GPS north position measurement noise as described in section
4.6.
vψGPS The GPS heading noise as described in section 4.6.
vvGPS The GPS heading noise as described in section 4.6.
vodometry (t) The odometry speed noise as described in section 4.6.
wEodometry The odometry east position system noise whose characteristics is
described in section 4.6.
wNodometry The odometry north potition system noise whose characteristics is
described in section 4.6.
wψodometry The odometry heading system noise whose characteristics is de-
scribed in section 4.6.
wψgyro The gyro heading system noise whose characteristics are described
in section 4.6.
vEGPS
˜
˜
ψ = arctan2( ) (4.5)
vNGPS
˜
—page 25—

34. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation
vGPS =
˜ v2 GPS + v2 GPS
˜E ˜N (4.6)
Where arctan2 is the four quadrant version of arctan. The inverse trigonometric
function arctan2(y, x) is defined as r = arctan(y/x) using:
if x = y = 0, then r = inde f inite
if x > 0 and y = 0, then r = 0
if x < 0 and y = 0, then r = π, else
if y < 0, then −π < r < 0
if y > 0, then 0 < r < π
The function is used to find the direction from one point to another in 2-dimension
Euclidean 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 used
in 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 equation
4.7.
   ˜ ˜   
E − ωr Rr +ωl Rr sin(ψ)
2 wEodometry
 ˜ ˜
  ωr Rr +ωl Rr cos(ψ)   wNodometry 
N
d 
 
=
2
˜ ˜
ωr Rr −ωl Rr
 
 +  wψ


ψ (4.7)
dt 

 
 
B  
 
odometry 

Rr 0 0
Rl 0 0
The state vector is denoted x1 . The east and north position, angle between the
north 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 output
function in equation 4.8.
     
˜
E E vEGPS
 N ˜  N   vNGPS 
y1 = 
˜ 
 ψ  = h1 (x1 ) + v(t) = 
 + 
  vψGPS  . (4.8)
˜ ψ
˜ ˜
ωr Rr +ωl Rl
vGPS
˜ 2
vvGPS
The prediction step is performed by solving the nonlinear equation 4.7 without the
noise term by using Euler integration (Euler integration is assumed sufficient since
the dynamics is relatively low and the update rate high). The covariance matrix
is propagated using the Ricatti equation whence the system function has to be lin-
earized according to equation 4.9.
—page 26—

35. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation
 ˜ ˆ ˜ ˆ

−ωr sin ψ −ωl sin ψ
0 ˜ ˆ
0 −v cos ψ 2 2
 ˜
ωr cos ψˆ ˜
ωl cos ψˆ 
ˆ
∂f1 
0 0 −v sin ψ
˜ 2
˜
2
˜


F1 = = 0 0 0 ωr
− ωl , (4.9)
∂ˆ 1 
x B B 
 0 0 0 0 0 
0 0 0 0 0
Further, the measurement function h1 is already linear and its Jacobian matrix is
 
1 0 0 0 0
∂h1  0 1 0 0 0 
H1 = = x1 = 
˜  0 0 1 0 0 .
 (4.10)
∂ˆx
˜
ωr ˜
ωl
0 0 0 2 2
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.
 
1 0 0 0 0
 0 1 0 0 0 
 
 0 0 1 0 0 
 
 0 0 0 ˜
ωr ˜
ωl 
H1  2 2 
= ˜ ˆ ˜ ˆ  (4.11)
H1 F1  0 0 −v cos ψ − ωr sin ψ
˜ ˆ 2 − ωl sin ψ
2 
 ˜ ˆ ˜ ˆ 

 0 0 −v sin ψ ωr cos ψ
˜ ˆ 2
ωl cos ψ
2


˜ ˜l
 0 0 0 ωr
B −ω B

0 0 0 0 0
As seen in equation 4.11 a matrix of full rank can be constructed using rows one
through four and row seven. The rest of the rows are not contributing to the matrix
˜ ˜
rank. One observations done is that if ωr and/or ωl are zero the matrix does not
have 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 wheel
radii cannot be estimated.
4.3 Odometry-GPS Estimating Wheel Radii and Rear Wheel
Axis Width
This model is taken, somewhat modified, from [4].
—page 27—

36. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation
In this system the GPS and the odometry is used. The odometry is used in the
Kalman 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 system
is as equation 4.12.
   ˜ ˜
  
E − ωr Rr +ωl Rl sin(ψ)
2 wEodometry
 N   ωr Rr +ωl Rl cos(ψ)   wNodometry
˜ ˜ 
   2   
d  ψ  
 ˜ ˜
ωr Rr −ωl Rl  
  wψodometry 
 = ˆ
B +  (4.12)
dt 
 Rr  
  0   0 

     
Rl 0  0
B 0 0
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 the
filter output function in equation 4.13.
     
˜
E E vEGPS
 ˜
N   N   vNGPS 
y2 (ˆ ) = 
˜ x 
 = h2 (x2 ) + v(t) = 
 
+ 
  vψGPS  (4.13)
˜
ψ ψ
˜ ˜
ωr Rr +ωl Rl
vGPS
˜ 2
vvGPS
The prediction step is performed by solving the nonlinear equation 4.12 without
the noise term by using Euler integration (Euler integration is assumed sufficient
since 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 be
linearized according to equation 4.14.
 ˜
−ωr sin ψˆ ˜
−ωl sin ψˆ 
0 ˜ ˆ
0 −v cos ψ 2 2 0
 0 ˆ
0 −v sin ψ
˜
˜
ωr cos ψˆ ˜
ωl cos ψˆ
0 
 2 2 
∂f2  0 0 0 ˜
ωr ˜
− ωˆl ˜ ˆ −˜ ˆ
− ωr RrB2ωl Rl


F2 = = Bˆ B ˆ  (4.14)
∂ˆ 2 
x 0 0 0 0 0 0 
 
 0 0 0 0 0 0 
0 0 0 0 0 0
Further, the measurement function h2 is linearized and its Jacobian matrix is
 
1 0 0 0 0 0
∂h2  0 1 0 0 0 0 
H2 = =  (4.15)
∂ˆ 2  0
x 0 1 0 0 0 
˜
ωr ˜
ωl
0 0 0 2 2 0
—page 28—

37. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation
4.3.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.16.
 
1 0 0 0 0 0
 0 1 0 0 0 0 
 
 0 0 1 0 0 0 
 
 0 0 0 ˜
ωr ˜
ωl
0 
H2  2 2 
= ˜ ˆ ˜ ˆ  (4.16)
H2 F2  0 0 −v cos ψ − ωr sin ψ
˜ ˆ 2 − ωl sin ψ
2 0 
 ˜ ˆ ˜ ˆ 
 0 0 −v sin ψ ωr cos ψ
˜ ˆ ωl cos ψ
0 
 2 2
˜ ˜ ˆ−˜ ˆ 
 0 0 0 ˜
ωr
− ωˆl − ωr RrB2ωl Rl 
Bˆ B ˆ
0 0 0 0 0 0
As seen in equation 4.16 all the columns are independent, thus the matrix has full
˜ ˜
rank. One observations that is done, is that if ωr and/or ωl are zero the matrix does
˜ ˆ ˜ ˆ
not have full rank thus is not observable. If ωr Rr = ωl Rl , then row seven, column
six is zero. Since this is the only term in column six the system is not observable
in this chase.
˜ ˜ ˜ ˆ ˜ ˆ
The observability analysis result is that if either ωr = 0, ωl = 0 or ωr Rr = ωl Rl the
system is not observable for this k. Otherwise the system is observable. ωr = 0˜
˜
and ωl = 0 corresponds to the vehicle having no velocity meaning that the two
˜ ˆ ˜ ˆ
wheel radius cannot be estimated. If ωr Rr = ωl Rl the vehicle is not turning, thus
the vehicle rear wheel axis width does not matter for the position or velocity of the
vehicle, it is not observable.
4.4 Odometry-Gyro-GPS estimating Gyro Bias and Scale
Factor
This model is taken, somewhat modified, from [4].
In this system the GPS, the gyro and the odometry is used. In the Kalman filter
prediction stage the odometry is used for vehicle velocity and the gyro for vehicle
heading and the GPS serves as measurements. Position, heading, gyro bias and
gyro scale factor is estimated. The gyro measurement model chosen is as equation
4.17.
r(t) − b
r(t) =
˜ + v1 (t) (4.17)
s
—page 29—

38. Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation
4.17 is a variable substitution from 4.18
r1 (t) = s1 r(t) + b1 + v2 (t)
˜ (4.18)
made to avoid nonlinearities in the covariance matrix 4.21 for the corresponding
elements. In the chase of this system the white noise v1 (t) becomes system noise
wψgyro . This gives the system given in equation 4.19.
   ˜ ˜   
E − ωr Rr +ωl Rl sin(ψ)
2 wEodometry
 N   ˜ r Rr +ωl Rl
ω ˜
cos(ψ)   wNodometry 
d 
 
=
2  
+

,
ψ −˜s − b
r wψgyro (4.19)
dt 

 
 
 
 


b 0 0
s 0 0
ψ is here the vehicle heading angle from north to west obtained from the gyro with
a negative sign. This because the gyro angular rate is defined from north to west
and 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 angle
between the north axis is used as measurements. The measurement function h3 (x3 )
is described with its noise in the filter output function in equation 4.20.
     
˜
E E vEGPS
y3 =  N  = h3 (x3 ) + v(t) =  N  +  vNGPS 
˜ ˜ (4.20)
˜
ψGPS ψ vψGPS
The prediction step is performed by solving the nonlinear equation 4.19 without
the noise term by using Euler integration (Euler integration is assumed sufficient
since 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 be
linearized according to equation 4.21.
 
0 ˜
0 −v cos ψ 0
˜ 0
 0 ˜
0 −v sin ψ 0
˜ 0 
∂f3  
F3 = ˜3 = 
=x  0 0 0 −1 −rgyro ,
 (4.21)
∂ˆ 3
x  
0 0 0 0 0
0 0 0 0 0
Further, the measurement function h2 is already linear and its Jacobian matrix is
 
1 0 0 0 0
∂h3 
H3 = = 0 1 0 0 0 . (4.22)
∂ˆ 3
x
0 0 1 0 0
—page 30—