CALIFORNIA STATE UNIVERSITY, NORTHRIDGE
LOCALIZATION USING KALMAN FILTER WITH GPS AND WHEEL ENCODERS
ON A BCI WHEELCHAIR
A graduate project submitted in partial fulfillment of the requirements
For the degree of Master of Science
In Electrical Engineering
By
Garrett Leonard
December 2014
Signature Page
The graduate project of Garrett Leonard is approved:
______________________________________
Dr. Ali Amini
_________________
Date
______________________________________
Dr. C.T. Lin
_________________
Date
______________________________________
Dr. Kourosh Sedghisigarchi, Chair
_________________
Date
California State University, Northridge
ii
Acknowledgement
I would like to thank Professor Kourosh Sedghisigarchi for agreeing to be the
committee chair for my project, providing his knowledge, and helping shape this project.
I would like to thank Professor C.T. Lin for his help and knowledge on
completing this project, and for letting me use the BCI Wheelchair for my project.
I would like to thank Professor C.T. Lin and Professor Ali Amini for agreeing to
be committee members for my project and their support.
I would like to thank the Department of Electrical and Computer Engineering for
their excellent graduate program.
I would like to thank my family and friends for their support through my graduate
program.
I would like to thank my girlfriend, Alfie Gil, for her continuing support and
encouragement in completing my Master’s Degree.
iii
Table of Contents
Signature Page
ii
Acknowledgement
iii
List of Figures
v
Abstract
vi
1.0
Introduction
1
Objective
2
Background
4
1.1
2.0
2.1
Wheelchair GPS Background
4
2.2
Kalman Filter Background
5
3.0
Wheelchair Modeling
8
4.0
Kalman Filter Integration
13
5.0
Results
17
6.0
Conclusion
26
References
28
Appendix A
29
Appendix B
31
iv
List of Figures
Figure 1 – Kalman Filter Equation Process
6
Figure 2 – Wheelchair Kinematic Model
10
Figure 3 – Overall System Flowchart
16
Figure 4 – First Run GPS vs Kalman Filter
18
Figure 5 – GPS with Added Noise vs Kalman Filter
20
Figure 6 – Matlab Kalman Filter and LabView Kalman Filter vs GPS with added Noise21
Figure 7 – Test Run with GPS in Poor Reception Area
22
Figure 8 – GPS with Manually added disturbances run 1
23
Figure 9 – GPS with Manually added disturbances run 2
24
Figure 10 - LabView Code Front Panel Snapshot
29
Figure 11 - LabView Code Block Diagram Snapshot
30
v
Abstract
LOCALIZATION USING KALMAN FILTER WITH GPS AND WHEEL ENCODERS
ON A BCI WHEELCHAIR
By
Garrett Leonard
Master of Science in Electrical Engineering
The objective of this project is to improve the accuracy of the Global Positioning
System (GPS) on the Brain Computer Interface (BCI) Wheelchair at California State
University, Northridge. In optimal conditions, the GPS can have an accuracy of one or
less meter radius around the wheelchair. These conditions require there to be no trees,
buildings, or other objects obstructing the antennas reception of the signals from the GPS
satellites. These conditions aren’t always available and there are times when there will be
trees and buildings near the wheelchair. When the wheelchair is not in optimal conditions
the accuracy degrades and goes above one meter radius around the wheelchair. This
inaccuracy makes it harder for the navigation system to give the wheelchair good
directions of where to go.
The solution being proposed to improve the accuracy of the GPS is a Kalman
filter. The other sensors available on the wheelchair that track the motion of the
wheelchair are the motor encoders. The data from the motor encoders and the GPS data
vi
are inputted into the Kalman filter to output more accurate data for the location of the
wheelchair. The motor encoders are not affected by the same elements as the GPS such as
trees and buildings. The main element that will affect the motor encoders is the wheels
loosing traction. This situation shouldn’t happen that often because the wheelchair is
intended to stay on smooth ground such as cement sidewalks.
A Kalman filter was developed to take GPS data and motor encoder data and
calculate an estimated position. To test the Kalman filter the wheelchair was taken
outside on predetermined routes to collect GPS data and motor encoder data. The data
was then filtered by the Kalman filter and the data was analyzed. Several different
methods of collecting data were taken. Some methods involved adding Gaussian white
noise into the GPS data and using the Kalman filter to remove the added noise. Another
method was to add noise in manually by blocking the GPS signal. The data analyzed in
this report shows that the Kalman filter proposed is able to successfully filter out the
noise in the position data from the GPS.
vii
1.0
Introduction
The goal of the Brain Computer Interface (BCI) wheelchair at California State
University, Northridge is to give more freedom to people with disabilities that normally
require help from other people. The BCI wheelchair at California State University,
Northridge was discussed by C.T. Lin, Craig Euler, Po-Jen Wang, and Ara Meckhtarian
[1]. The BCI wheelchair has the ability to move autonomously in its environment while
avoiding obstacles. This makes it easier on the user so they do not have to constantly give
the wheelchair commands. In an indoor setting, the wheelchair is confined by the walls of
the building and the obstacles in the enclosed area. This gives the wheelchair less routes
it can possible travel making it so the user can give minimal commands. In an outdoor
setting, the wheelchair has more freedom of movement. This can make it harder on the
user because they will have to give more commands to the wheelchair in order to go to
their destination. This is because there aren’t as many boundaries confining the
wheelchair. This gives the wheelchair the behavior that it’s wandering because it doesn’t
have a goal. This is where the GPS comes into the equation. Using the GPS, a destination
can be set and the A* algorithm used on the wheelchair is able to calculate an optimal
path to the destination using predefined latitude and longitude points (waypoints).
The navigation system on the wheelchair is able to guide it through the path
calculated by the A* algorithm as long as the GPS has good accuracy. A* is an algorithm
that processes through all the preset waypoints and calculates the path with the shortest
distance. It requires waypoints to be preset of all the possible destinations and if needed
any waypoints in between destinations to make the path smoother. The algorithm is
efficient in the way that it doesn’t need to calculate all the possible paths and so it finds
1
the shortest path in minimal time. When the accuracy of the GPS degrades, the navigation
system has a hard time telling where it is and where it needs to go; so, it has a hard time
following the calculated path. The navigation system might think the wheelchairs next
waypoint is in the grass next to the sidewalk or worse in the street. These types of
situations need to be avoided in order to protect and help the user navigate in an outdoor
environment.
1.1
Objective
The main objective of this project is to improve the accuracy of the position data
of the wheelchair. To improve the accuracy of the position data when the GPS’s accuracy
is degrading, another sensor is needed. Another sensor will tell us the movement of the
wheelchair when the GPS cannot because of noise in the data. There are many different
types of sensors that can be used to detect movement, one being motor encoders. An
encoder is a sensor that detects the movement and position of the motor. So a motor
encoder can give information such as how fast the motor is spinning and how many
rotations it has done. To combine the data from the two different sensors to get more
accurate position data, a Kalman filter is used. The Kalman filter takes the data from both
of the sensors and filters out the noise to produce more accurate position data.
Kalman filters have been proposed in many different publications for the purpose
of removing noise and is widely used with GPS. Brian W. Tolman [2] proposed a dual
frequency GPS with Kalman filter to remove noise from the raw GPS signals and
improve the accuracy of the GPS to 2-3 centimeters. This proposed idea doesn’t require
other sensors but instead filters out noise in the signals being sent to the GPS that can be
susceptible to interference. Daniel Maier and Alexander Kleiner [3] proposed the idea of
2
creating a 3D map of the urban environment around the robot and using that data to
figure out what satellites were obstructed. The data along with data from an IMU, GPS
and odometry were used in a Kalman filter to get better position accuracy in urban
environments. The idea of proposing three different types of sensor integration schemes
with the Kalman filter was proposed by J Gao, M.G. Petovello, and M.E. Cannon [4].
The three different types of sensor integration schemes proposed were GPS/INS/Wheel
speed sensor, GPS/INS/Yaw rate sensor, and GPS/INS/Yaw rate sensor/Wheel speed
sensor. INS stands for Inertial Navigation System and they used a tactical grade HG1700
IMU for that purpose. Rudy Negenborn [5] discussed the difficulties in robot localization
and different methods for using the Kalman filter. One localization method he discussed
using the Kalman filter is detecting landmarks to detect the location of the robot. He
points out the difficulties with using GPS and other sensors used for localization. A
Kalman filter is proposed by Jonas Baumann [6] for a robot with similar sensors as the
wheelchair to determine its location. The robot is a land mine detecting robot that they
want to move autonomously. In all these examples, the Kalman filter is widely used for
GPS and has been proven to improve position data accuracy.
3
2.0
2.1
Background
Wheelchair GPS Background
The GPS gives the wheelchair the ability to navigate through outdoor
environments more successfully. Without the GPS the wheelchair doesn’t have a global
heading and only has a local heading. The local heading is determined by the obstacle
avoidance algorithm on the wheelchair. Only having a local heading means that the
wheelchair only decides its direction of travel based on the immediate environment
around it. While not having a global heading means that the wheelchair doesn’t have a
goal or destination. If the wheelchair does not have a global heading, then without having
a predetermined environment, it’s hard to determine where the wheelchair will end up
going. Having a global heading gives the wheelchair the ability to navigate to a
destination in an environment that is not completely predetermined.
GPS fixes the problem of the wheelchair not having a global heading but it comes
with some flaws. It is easily susceptible to lose in accuracy of the position data in an
urban environment. It needs to be able to receive signals from multiple satellites in order
to give position data. The minimum number of satellites a GPS needs is four [7]. Using a
process called trilateration, GPS is able to calculate its position. Trilateration is the
process of measuring distances to find a location using circles and spheres. With GPS
you measure the distance from one satellite giving you a sphere around the satellite of all
possible locations. The distance is then measured from another satellite and the sphere
from this satellite will intersect with the other sphere giving a circle. The distance to a
third satellite is used to make another sphere which intersects with the circle in two spots.
Measuring the distance from a forth satellite will then create a sphere which intersects
4
with one of the two points giving the approximate location. . Four satellites is the
minimum number, but it’s not the ideal number. Only having four satellites will give an
estimated position with inaccuracies in the meters. This amount of error is unacceptable
for use on the wheelchair. Ideally, the GPS should be able to receive signals from as
many satellites as possible. The Air Force set up a 27-slot constellation of GPS satellites
where there are at least four in the sky at once anywhere in the world [8]. In an urban
environment there are many sources that can block the signal from the satellites such as
trees and buildings.
2.2
Kalman Filter Background
A Kalman filter is an algorithm that takes in data from multiple measurement
sources and estimates the state of the system. In the Kalman filter, an estimate of the state
of the system is produced by taking prior knowledge of the system and measuring devices
and current measurement data. The Kalman filter is considered to be a discrete linear
recursive optimal estimation algorithm. The word optimal is used when the system is
linear and the measurement noise is white and Gaussian though the Kalman filter is still a
good estimation of the system without all these conditions [9]. The term recursive comes
from the Kalman filter using only the current and the previous data instead of storing all
the measurement data to estimate the next state. According to the reference in [10], the
Kalman filter is able to give an estimate of the state even if the exact model of the system
is not known. In the end, the Kalman filter is able to take data from multiple
measurement sources; and given the accuracy of each measurement device, it’s able to
combine all the data and estimate the state of the system.
5
A Kalman filter goes through two stages of computation, the “Time Update” stage
and the “Measurement Update” stage. The “Time Update” stage is essentially the
prediction stage and the “Measurement Update” is the correction stage [10]. This process
goes through the five equations shown in Figure 1.
Measurement Update (Correct)
Time Update (Predict)
1. Compute the Kalman gain
𝐾𝑘 = 𝑃𝑘− 𝐻 𝑇 𝐻𝑃𝑘− 𝐻 𝑇 + 𝑅 −1
2. Update estimate with measurement 𝑧𝑘
1. Project the state ahead
𝑥̂𝑘−
= 𝐴𝑥̂𝑘−1 + 𝐵𝑢𝑘−1
2. Project the error covariance ahead
𝑃𝑘−
𝑥̂𝑘 = 𝑥̂𝑘− + 𝐾𝑘 𝑧𝑘 − 𝐻𝑥̂𝑘−
3. Update the error covariance
𝑃𝑘 = 𝐼 − 𝐾𝑘 𝐻 𝑃𝑘−
𝑇
= 𝐴𝑃𝑘−1 𝐴 + 𝑄
Initial estimates for 𝑥̂𝑘−1 and 𝑃𝑘−1
Figure 1 – Kalman Filter Equation Process
Where:
𝑥̂𝑘 : The states of the system
𝑥̂𝑘− : The predicted system states based on previous states and inputs
A and B: Are determined based on the system
𝑢𝑘 : The input to the system
𝑃𝑘 : Error Covariance
𝑃𝑘− : The predicted error covariance based on the previous error covariance and
process noise covariance
Q: Process noise covariance
R: Measurement noise covariance
K: Kalman gain
H: Relates the state to the measurement 𝑧𝑘
𝑧𝑘 : Measurements
6
The Kalman filter can be used in many different applications such as computer
vision and radar tracker. Erik Cuevas, Daniel Zaldivar, and Raul Rojas [11] proposed
using the Kalman filter for vision tracking. They used the Kalman filter to track moving
objects in computer vision. The Kalman filter can be used for frequency estimation of
distorted power system signals as proposed by P.K. Dash, A.K. Pradhan, and G. Panda
[12]. The state of interest in these examples is not known but the system is observable; so
using sensors, the Kalman filter can estimate the state of the system. For this project, the
Kalman filter will be used to estimate the position of the wheelchair using measurement
data from the GPS and motor encoders.
7
3.0
Wheelchair Modeling
A kinematic model of the wheelchair is needed in order to analyze the movement
of the wheelchair. The kinematic model can translate the wheel speeds into position data
of the wheelchair. In order to make sense of the position data from the kinematic model
and compare it to the GPS data, a similar coordinate system is needed. For this project the
X and Y planes of the Cartesian coordinate system are used. The GPS data will also need
to be converted to the X and Y Cartesian planes. The data that is read from the motor
encoders is the rotational speed of the motors. The rotational speed read from the
encoders is in a proprietary unit to the manufacturer called SilverLode Actual Velocity
(SAV). It’s desired to have a linear speed in meters per second (m/s) for each wheel. The
first step to get the linear speeds is to convert from SAV to counts per second (CPS). A
count is a unit of measure that tells how much a motor has rotated. The next step is to
convert from CPS to revolutions of the motor per second. For the motors on the
wheelchair, there are 16000 counts in one revolution. The next step is to convert from
revolutions of the motor per second to revolutions of the wheel per second. The gearbox
on the wheelchair has a ratio of 18 to 1, meaning 18 motor revolutions to one wheel
revolution. Finally, the revolutions of the wheel per second can be converted to m/s,
given the radius of the wheel 0.17272 meters (m). Equation (1) below is used for the
conversion from SAV to m/s.
8
𝑉 =
𝑚
𝑠
𝑉𝑆𝐴𝑉 ∗32.55307677
16000
18
∗ 2𝜋 ∗ 0.17272
(1)
Where:
1 SAV = 32.55307677 CPS
1 Motor Revolution = 16000 counts
1 Wheel Revolution = 18 Motor Revolutions
Radius of the wheel = 0.17272 m
The next step is to convert the linear speeds from the motors into movement of
the wheelchair. The wheelchairs drive system consists of a differential drive with two
caster wheels. The wheelchair sits on four wheels, two being drive wheels and two being
caster wheels. The motors controlling the wheels are independent so this makes the
wheelchair have a differential drive. The first part when making the kinematic model is to
establish the orientation of the X and Y axes of a fixed inertia coordinate frame. The X
and Y axes were oriented so that the positive X axis is pointing to 0°. This gives θ, the
counterclockwise angle from the positive X axis to the direction of the wheelchair, ω, the
angular velocity of the wheelchair, v, the linear velocity of the wheelchair, 𝑥̇ , the velocity
along the X axis, and 𝑦̇ , the velocity along the Y axis [13]. The X and Y axes and the
representation of the wheelchair are shown in Figure 2. In Figure 2, the gray circles
represent the caster wheels, the gray rectangles are the drive wheels with the circle
between them is the center of rotation.
9
Y
v
𝑦̇
θ
X
𝑥̇
ω
Figure 2 – Wheelchair Kinematic Model
The next step would be to calculate 𝑥̇ and 𝑦̇ , and the equations to calculate 𝑥̇ and
𝑦̇ are shown below.
𝑥̇ = 𝑣𝑐𝑜𝑠 𝜃
𝑦̇ = 𝑣𝑠𝑖𝑛 𝜃
𝜃̇ = 𝜔
(2)
Where:
v: linear velocity of the wheelchair, described with respect to the moving
coordinate frame
ω: angular velocity of the wheelchair, described with respect to the moving
coordinate frame
θ: heading of the wheelchair measured counterclockwise from the X axis
10
The value that is actually desired is the position of the wheelchair and not the
velocities. So the equations above need to be translated from continuous time to discrete
time giving the equations shown below.
𝑥 𝑡 + 1 = 𝑥 𝑡 + 𝑣𝑐𝑜𝑠 𝜃 ∙ ∆𝑡
𝑦 𝑡 + 1 = 𝑦 𝑡 + 𝑣𝑠𝑖𝑛 𝜃 ∙ ∆𝑡
𝜃 𝑡 + 1 = 𝜃 𝑡 + 𝜔 ∙ ∆𝑡
(3)
Where:
∆𝑡: change in time
These equations will tell us the X and Y position of the wheelchair from when 𝑡 =
0. In order to use these equations in the Kalman filter, they need to be represented in
matrix form giving the equations shown below.
𝑥 𝑡+1
1
[𝑦 𝑡 + 1 ] = [0
𝜃 𝑡+1
0
𝑐𝑜𝑠 𝜃 ∙ ∆𝑡
0 0 𝑥 𝑡
𝑦
𝑡
]
[
]
+
[
1 0
𝑠𝑖𝑛 𝜃 ∙ ∆𝑡
0 1 𝜃 𝑡
0
0
𝑣
0 ] [𝜔 ]
∆𝑡
(4)
The equations above have the inputs linear velocity v and angular velocity ω. The
linear velocity and angular velocity need to be translated into the wheel speeds for both
of the wheels 𝑉𝐿 and 𝑉𝑅 . To calculate 𝑉𝐿 and 𝑉𝑅 the equations below were used.
𝑉 −𝑉
𝜃̇ = 𝑅 𝐿 𝐿
𝑣=
𝑉𝑅 +𝑉𝐿
(5)
2
Where:
𝜃̇ = 𝜔
L (the length between the wheels) = 0.6985 m
11
The set of equations, Equation 5, give the transformation matrix shown below.
1
1
𝑉
𝑣
[ ] = [21 2 1] [ 𝑅 ]
𝜔
− 𝐿 𝑉𝐿
𝐿
(6)
The transformation matrix, Equation 6, gives the following formula.
1
𝑥 𝑡+1
1
[𝑦 𝑡 + 1 ] = [0
𝜃 𝑡+1
0
0 0 𝑥 𝑡
1 0] [ 𝑦 𝑡 ] +
0 1 𝜃 𝑡
2
1
2
[
𝑐𝑜𝑠 𝜃 ∙ ∆𝑡
𝑠𝑖𝑛 𝜃 ∙ ∆𝑡
1
0.6985
∆𝑡
1
2
1
𝑐𝑜𝑠 𝜃 ∙ ∆𝑡
𝑉 𝑡
𝑠𝑖𝑛 𝜃 ∙ ∆𝑡 [ 𝑅 ] (7)
2
𝑉𝐿 𝑡
1
− 0.6985 ∆𝑡 ]
The equation above, Equation 7, is the final equation required for the kinematic
model of the wheelchair. This equation gives the position of the wheelchair depending on
the time, orientation, and wheel speeds.
12
4.0
Kalman Filter Integration
A Kalman filter estimates the state of the defined system by knowing the model of
the system and knowledge of the measurement device [9]. A model of the system is
needed in order for the Kalman filter to estimate the state. Part of the model was made in
the previous chapter for the kinematic model. The kinematic model of the wheelchair is
able to calculate the movement of the wheelchair, so it can be used as the system model
with some small modifications. The kinematic model calculated earlier only has the input
part of the system, that is, matrices A and B of the system state equation. For the output
part of the system, matrices C and D are not needed for the Kalman filter. For the
Kalman filter, matrix H (in Figure 1) needs to be determined and this matrix needs to
relate the state to the measurement data. The states that need to be estimated are x and y
since they relate to the GPS position data. The H matrix that relates the GPS data to the x
and y data of the kinematic model is shown below.
𝐻=[
1 0 0
]
0 1 0
(8)
The H matrix, Equation 8, gives us the model used in the Kalman filter shown
below in Equation 9.
13
𝑥 𝑡+1
1
[𝑦 𝑡 + 1 ] = [0
𝜃 𝑡+1
0
𝑧1
1 0
[𝑧 ] = [
2
0 1
1
𝑐𝑜𝑠 𝜃 ∙ ∆𝑡
2
𝑥
𝑡
0 0
1
1 0] [𝑦 𝑡 ] + 𝑠𝑖𝑛 𝜃 ∙ ∆𝑡
2
0 1 𝜃 𝑡
1
[ 0.6985 ∆𝑡
1
𝑐𝑜𝑠 𝜃 ∙ ∆𝑡
2
1
𝑉𝑅 𝑡
𝑠𝑖𝑛 𝜃 ∙ ∆𝑡 [ 𝑉 𝑡 ]
2
𝐿
1
−
∆𝑡
0.6985 ]
𝑥 𝑡
0 𝑦 𝑡
][
]
0
𝜃 𝑡
(9)
Where:
𝑧1 : Calculated GPS measurement of the x position
𝑧2 : Calculated GPS measurement of the y position
The matrices Q and R of the Kalman filter, need to be determined, refer to Figure
1. The matrix Q is the process noise covariance matrix, which represents any noise in the
system model [10]. So, this would be any noise in the Kinematic model or the motor
encoders. For the purpose of this project this noise is assumed to be very small because
the wheelchair is not moving fast and is designed to not slip. The error would mostly be
coming from the GPS due to signal obstructions. The matrix R is the measurement noise
covariance matrix, which represents any noise in the measurement device [10]. So, this
would be the noise in the GPS, which is the noise that is to be filtered. The matrix R will
be calculated by comparing the Kalman filter output to a reference, which is the desired
position data. By adjusting the matrix R, the output of the Kalman filter can be adjusted
to be as close to the reference as possible. This method is used because the accuracy of
the GPS changes based on the environment. So, it is hard to determine R.
To calculate the B matrix for the Kinematic model and Kalman filter, the angle θ
needs to be known. The B matrix relates the velocity inputs to the states of the system,
14
which are the position data. The angle, θ, is a counterclockwise angle measured starting
from the positive X axis. To get the current θ value, the compass heading is used. The
compass heading is a clockwise angle with respect to north being 0°. The X and Y
coordinate system for the GPS data and the kinematic model need to be oriented the
same. The compass data is oriented with north 0° being on the positive Y axis, while the θ
value from the kinematic model is oriented with 0° being on the positive X axis. With the
X and Y axes being oriented the same, the angles are still off by 90° and the compass
heading increments clockwise while the θ from the kinematic model increments
counterclockwise. The conversion shown below is used to convert the compass heading
to the θ value used in the kinematic model.
𝜃 = −𝜙 + 90°
𝑖𝑓 𝜃 < 0 𝑡ℎ𝑒𝑛 𝜃 = 𝜃 + 360°
(10)
Where:
φ = Compass Heading
θ = Kinematic Model Heading
The only values in the Kalman filter equations left to be calculated are the P and
K matrices. The matrices P and K will be calculated in the Kalman filter process shown
in Figure 1. The matrix P is the estimate error covariance and the matrix K is the Kalman
gain. Now that all of the values needed have been calculated, the Kalman filter then goes
through the process shown in Figure 1. The process shown in Figure 1 is repeated until
all the data points have been processed, outputting the estimated position data matrix
𝑥̂ 𝑡 that contains x and y position data. All the elements of the Kalman filter are now
15
accounted for. To show how the Kalman filter fits into the wheelchairs control process a
flowchart is shown in Figure 3.
Figure 3 – Overall System Flowchart
Where:
𝑥𝐷 𝑎𝑛𝑑 𝑦𝐷 : Desired X and Y position
𝜃𝑀 : Current heading of wheelchair measured from compass
𝑋𝑀 𝑎𝑛𝑑 𝑌𝑀 : Current GPS measurement data
𝜃𝐷 : Desired heading to reach desired position data
𝑉𝑅𝐷 𝑎𝑛𝑑 𝑉𝑅𝐿 : Desired wheel speeds to reach desired position
𝑉𝑅 𝑎𝑛𝑑 𝑉𝐿 : Measured wheel speeds from encoders
𝑥̂𝑘− , 𝑦̂𝑘− , 𝑎𝑛𝑑 𝜃̂𝑘− : Calculated position data from kinematic model
𝑥̂𝑘 𝑎𝑛𝑑 𝑦̂𝑘 : Kalman filter estimated position data
16
5.0
Results
After designing the Kalman filter, it needs to be tested in order to make sure it
works as intended. The first idea to test the Kalman filter was to collect GPS, compass,
and motor encoder data to simulate the Kalman filter. The data collected from the
wheelchair would be run through the Kalman filter and the output would be compared to
the GPS data and kinematic model data. Based on the results, the R covariance matrix
could be changed to try and tune the filter. The Kalman filter would be rerun and the
results would be reanalyzed.
The Kalman filter process was programmed in LabView since it’s the
programming language used in all the programs that run on the wheelchair, and it has a
built-in Kalman filter function. The built-in function in LabView makes it easier to
program the Kalman filter for the first time. The wheelchair was manually run to have
better control of the wheelchair movement, through a predetermined path. After
collecting the GPS data, it was then inputted into the Kalman filter program, and it was
rerun until it looked like the noise was removed. A graph of the GPS data and Kalman
filter data of the first run is shown in Figure 4.
17
Test Run 1
N
0
-5
-10
Y (m)
-15
-20
GPS Data
-25
Kalman Filter Data
-30
-35
-40
-45
-5
0
5
10
15
20
25
30
35
40
45
50
55
60
X (m)
Figure 4 – First Run GPS vs Kalman Filter
The run in Figure 4 above starts at (0, 0) on the graph and ends near (58, -42).
After analyzing this data, it showed that the Kalman filter was doing something but it
wasn’t apparent if it was filtering out the noise or not since there was no reference to
compare to. A reference would be data that has no error that can be used for comparison.
It was apparent that a reference needed to be added to the graph in order to properly
compare the data from the Kalman filter.
The next idea was to add noise to the GPS data in the LabView program and use
the original GPS data as a reference. The noise added to the GPS data in the LabView
program was Gaussian white noise. Now the objective of the Kalman filter was to filter
out the added noise from the GPS data, so that the Kalman filter output should be similar
to the original GPS data. After tuning the first run, shown in Figure 4, it became obvious
18
that manually tuning was inefficient. So, it was decided to make an optimization
algorithm to compare the Kalman filter output to the reference signal. The algorithm
would compare the two sets of data and change the covariance matrix R until the total
linear difference between all the x values in both sets of data was minimized and the
linear difference between all the y values in both sets of data was minimized. The
equations to be minimized are shown as Equation 11.
𝑀𝑖𝑛 ∑ |𝑥𝑟𝑒𝑓 − 𝑥𝐾𝑎𝑙𝑚𝑎𝑛 |
𝑀𝑖𝑛 ∑ |𝑦𝑟𝑒𝑓 − 𝑦𝐾𝑎𝑙𝑚𝑎𝑛 |
(11)
The optimization algorithm was used with the new set of data shown in Figure 5.
A graph of the original GPS data, GPS data with noise added, and the Kalman filter is
shown below in Figure 5.
19
LabView Kalman Filter with Added Noise
N
-3
-13
Y (m)
-23
GPS Data
-33
GPS with Noise
LabView Kalman Filter
-43
-53
-63
-25
-20
-15
-10
-5
0
5
10
X (m)
Figure 5 – GPS with Added Noise vs Kalman Filter
The run in Figure 5 starts at (0, 0) on the graph and ends at around (-23, -60). The
GPS data is in black, the GPS with noise added is shown in the red dots, and the Kalman
filter output is shown in green. After analyzing the data, it showed that the Kalman filter
was actually removing the noise and looked similar to the original GPS data.
The next step was to double check the Kalman filter programmed in LabView to
make sure it was functioning as intended. The Kalman filter was then programmed in
Matlab as well to compare to the Kalman filter programmed in LabView. To test the
Kalman filter programmed in Matlab, the data shown in Figure 5 was used. The results
from the Matlab Kalman filter are shown below in Figure 6.
20
LabView and Matlab Kalman Filter with Added Noise
N
-3
-13
Y (m)
-23
GPS Data
GPS with Noise
-33
LabView Kalman Filter
Matlab Kalman Filter
-43
-53
-63
-25
-20
-15
-10
-5
0
5
10
X (m)
Figure 6 – Matlab Kalman Filter and LabView Kalman Filter vs GPS with added Noise
Just as in Figure 5, the graph in Figure 6 starts at (0, 0) on the graph and ends at
around (-23, -60). The GPS data is in black, the GPS with noise added is shown in the red
dots, the LabView Kalman filter output is shown in green, and the Matlab Kalman filter
is shown in blue. After analyzing the data, it can be seen that the Kalman filter in either
language, LabView or Matlab, behaves the same since the outputs are identical.
The next idea to test the Kalman filter was to collect more GPS data, but
purposely take the wheelchair through an area that would have obstructions that would
cause the GPS to degrade in accuracy. GPS data from a predetermined path was taken
and run through the Kalman filter using the same process as before. The optimization
21
algorithm wasn’t used for this set of data, since there was no reference data. The results
from this new setup data is shown in Figure 7.
5
Test Run with GPS in Poor Reception Area
N
0
-5
-10
Y (m)
-15
GPS Data
-20
LabView Kalman Filter
-25
Matlab Kalman Filter
-30
-35
-40
-45
-10
0
10
20
30
40
50
60
70
X (m)
Figure 7 – Test Run with GPS in Poor Reception Area
The run in Figure 7 starts at (0, 0) in the graph and ends near (58, -38). The GPS
data is in black, the LabView Kalman filter is in orange, which is made bolder on purpose
so that it can be seen, and the Matlab Kalman filter is in blue. After analyzing the data it
can be seen that the Kalman filters do filter out some of the noise in the center of the
graph. The center of the graph is where the GPS’s accuracy degraded due to obstructions.
One last proposal for testing the Kalman filter was to first collect some GPS data
of a predetermined path that has almost no obstructions to affect the GPS. The next part
22
was to rerun the same predetermined path but this time the GPS would be forced to loose
satellite signals once in a while by adding obstructions around the antenna. The data
collected was then inputted into the Kalman filter and the optimization algorithm was
used to tune it. The output for this test is shown in Figure 8.
10
GPS with Human made Interference
N
0
Y (m)
-10
-20
LabView Kalman Filter
-30
GPS data
Reference
-40
-50
-60
-10
0
10
20
30
40
50
60
70
X (m)
Figure 8 – GPS with Manually added disturbances run 1
The run in Figure 8 starts at (0, 0) in the graph and ends near (30, -56). The GPS
data with manually added disturbances is in orange, the reference is in black, and the
Kalman filter output is in blue. After analyzing the data it can be seen that the Kalman
filter removes most of the noise.
Another run where noise was manually added into the GPS data was taken and is
shown below in Figure 9.
23
10
GPS with Human made interference
N
0
-10
Y (m)
-20
Kalman Filter
-30
Reference
-40
GPS with Disturbances
-50
-60
-70
-10
0
10
20
30
40
50
X (m)
Figure 9 – GPS with Manually added disturbances run 2
The run in Figure 9 starts at (0, 0) in the graph and ends near (43, -58). The GPS
with manually added disturbances is in orange, the reference is in black, and the Kalman
filter output is in blue. After analyzing the data it can be seen that the Kalman filter does
remove most of the noise.
One problem that I ran into when collecting the data shown above was that the
compass was out of calibration. It was out of calibration because of the spot it was
located on the wheelchair has ferrous metal near it. The wheelchair is mostly made of
steel, and so it interferes with the compass since it’s magnetic. The procedure to calibrate
the compass was found on the company’s website and was used to calibrate the compass.
24
After being calibrated, the compass wasn’t allowed to be relocated anywhere on the
wheelchair, or else it would have to be recalibrated again. Even with the compass being
calibrated it became apparent that metal objects that the wheelchair drove by would
interfere with the compass. Paths were chosen that would try to avoid large ferrous metal
objects such as man holes or street signs, as much as possible. It was not possible to
completely avoid them, so in some of the runs it became more noticeable the compass
readings had interference. In these cases, the compass readings were adjusted to try and
remove any interference. This was possible since most of the time the wheelchair was
moving in a straight line at a relatively slow speed. The last set of data, shown in Figure
8, had some of the most interference from the compass out of the data shown in all the
figures. I believe that is part of the reason why the Kalman filter output was not able to
match the reference better.
25
6.0
Conclusion
After collecting all the GPS data shown above and filtering it with the Kalman
filter, it became apparent that the Kalman filter was improving the position data of the
wheelchair. The results of the data collected during the project show that the Kalman
filter should help improve the accuracy of the position data of the wheelchair. The next
steps would be to completely implement the Kalman filter on the wheelchair since only
pre-determined simulations were run in this project. After filtering all the different sets of
data, I noticed that the covariance matrix R did not always contain the same values for
different sets of data. The GPS gives more data than just the position, and one of them is
the accuracy of the GPS signal and the number of satellites it’s receiving data from. A
future improvement on this Kalman filter would be to use the accuracy data, and maybe
the number of satellites that the GPS is receiving signal from to dynamically change the
value of the covariance matrix R. P.J. Escamilla-Ambrosio and N. Mort [14] proposed
the idea of using fuzzy logic to implement an adaptive Kalman filter. Fuzzy logic deals
with logic that does not have a crisp value of 0 and 1, but can vary between 0 and 1. In a
broad sense, fuzzy logic is a combination of techniques and functions such as fuzzy sets,
membership, linguistic variables, fuzzy operations, and if-then rules, but not limited to
these. They were successfully able to implement the adaptive Kalman filter and show
results that it performed better than a normal Kalman filter.
The θ value outputted from the Kalman filter wasn’t actually used since the main
point of the project was to improve the accuracy of the x and y position data. After
experiencing the problems with the compass, I think the value could have been used to
help remove any interference the compass might get from large ferrous metal objects the
26
wheelchair drives by. In the future, the compass data can be inputted into the Kalman
filter with the GPS data and compared with the data from the kinematic model.
27
References
[1] C.T. Lin, C. Euler, P.-J. Wang and A. Mekhtarian, "Indoor and Outdoor Mobility for an
Intelligent Autonomous Wheelchair," International Conference on Computers Helping
People with Special Needs (ICCHP) Proceedings, Northridge, 2012.
[2] B. W. Tolman, "GPS Precise Absolute Positioning via Kalman Filtering," ION GNSS 21st.
International Technical Meeting of the, Austin, 2008.
[3] D. Maier and A. Kleiner, "Improved GPS Sensor Model for Mobile Robots in Urban Terrain,"
IEEE, Freiburg, 2010.
[4] J. Gao, M. Petovello and M. Cannon, "Development of Precise GPS/INS/Wheel Speed
Sensor/Yaw Rate Sensor Integrated Vehicular Positioning System," ION NTM, Monterey,
2006.
[5] R. Negenborn, "Robot Localization and Kalman Filters On finding your position in a noisy
world," Utrecht University, Utrecht, 2003.
[6] J. Baumann, "GPS-based autonomous point-to-point driving of all-terrain demining vehicle
GRYPHON," Swiss Federal Institute of Technology Zurich, Zurich, 2011.
[7] J. R. Clynch, "The Global Positioning System," 5 February 2003. [Online]. Available:
http://www.oc.nps.edu/oc2902w/gps/gpsoview.htm. [Accessed October 2014].
[8] "Space Segment," 30 October 2014. [Online]. Available:
http://www.gps.gov/systems/gps/space/. [Accessed 16 November 2014].
[9] P. S. Maybeck, Stochastic Models, Estimation, and Control Volume 1, New York: Academic
Press, Inc., 1979.
[10] G. Welch and G. Bishop, "An Introduction to the Kalman Filter," Department of Computer
Science University of North Caroline at Chapel Hill, Chapel Hill, 2006.
[11] E. Cuevas, D. Zaldivar and R. Rojas, "Kalman filter for vision tracking," Freie University
Berlin, University of Guadalajara, Berlin, 2005.
[12] P. Dash, A. Pradhan and G. Panda, "Frequency estimation of distorted power system signals
using extended complex Kalman filter," IEEE, Rourkela, 1999.
[13] R. Siegwart, I. R. Nourbakhsh and D. Scaramuzza, Introduction to Autonomous Mobile
Robots, Cambridge: The MIT Press, 2011.
[14] P. Escamilla-Ambrosio and N. Mort, "Adaptive Kalman Filtering Through Fuzzy Logic,"
University of Sheffield, Sheffield.
28
Appendix A
LabView Kalman Filter
Figure 10 - LabView Code Front Panel Snapshot
29
Figure 11 - LabView Code Block Diagram Snapshot
30
Appendix B
Matlab Kalman Filter Code
%%Read Data
Data = xlsread('G:\Graduate Project\Log\log 2014-04-23 19-29-57 Noise.xlsx');
dt = Data(:,3)/1000;
z = [Data(:,4) Data(:,5)]';
u = [Data(:,9) Data(:,10)]';
A = [1 0 0; 0 1 0; 0 0 1];
C = [1 0 0; 0 1 0];
Q = [1E-9 0 0; 0 1E-9 0; 0 0 1];
R = [1E-7 0; 0 1E-7];
I = eye(3);
dim = size(Data);
N = dim(1);
%Correct Heading from Compass Heading
for i=1:N
theta(i) = -Data(i,6) + 90;
if(theta(i) < 0)
theta(i) = theta(i) + 360;
end
end
%Initilize Kalman Filter
X(:,1) = [z(:,1); theta(1)];
31
P_n = [1E-9 0 0; 0 1E-9 0; 0 0 1];
for i=1:N
B = [.5*dt(i)*cos(degtorad(theta(i))) .5*dt(i)*cos(degtorad(theta(i)));
.5*dt(i)*sin(degtorad(theta(i))) .5*dt(i)*sin(degtorad(theta(i))); .5*dt(i) -.5*dt(i)];
%%Kalman Filter Equations
%Prediction
if i == 1
X(:,i) = A*X(:,i) + B*u(:,i);
else
X(:,i) = A*X(:,i-1) + B*u(:,i);
end
P = A*P_n*A' + Q;
%Correction
K = P*C'*inv(C*P*C' + R);
X(:,i) = X(:,i) + K*(z(:,i) - C*X(:,i));
P = (I - K*C)*P;
P_n = P;
end
32