Relative Distance Measurement Using GPS and Internal Vehicle Sensors

RelativeDistanceMeasurement
usingGPSandInternalVehicleSensors
MasterofScienceThesisintheMastersProgrammeCommunicationEngineering
AHMETIRKIN
SERKANKARAKIS
DepartmentofSignalsandSystems
DivisionofSignalProcessingandBiomedicalEngineering
CHALMERSUNIVERSITYOFTECHNOLOGY
Gteborg,Sweden2011
MastersThesis2011:15

MASTERSTHESIS2011:15
RelativeDistanceMeasurement
usingGPSandInternalVehicleSensors
AHMETIRKIN
SERKANKARAKIS
CHALMERSUNIVERSITYOFTECHNOLOGY
Gteborg,Sweden2011
RelativeDistanceMeasurementusingGPSandInternalVehicleSensors
AHMETIRKIN
SERKANKARAKIS
AHMETIRKIN,SERKANKARAKIS,2011
Examensarbete/InstitutionenfrSignalerochSystem,
Chalmerstekniskahgskola,2011:15
ChalmersUniversityofTechnology
SE41296Gteborg
Sweden
Telephone:+46(0)317721000
Cover:
AvisualoftheVehicletoVehiclecommunications,ofwhichthisthesisworkpresents
oneofthebenefits:relativepositioningoftargetvehicles.
DepartmentofSignalsandSystemsGteborg,Sweden2011
Kommentar [mp1]: Ifthereportis

printedbyforinstanceChalmers
reproservice,thisnameshouldbeinserted
here.Ifelsethedepartmentnameshould
begiven.

RelativeDistanceMeasurementusingGPSandInternalVehicleSensors
AHMETIRKIN
SERKANKARAKIS
ChalmersUniversityofTechnology
ABSTRACT
Each year a million people are being injured in traffic accidents. New active safety systems
have recently been introduced on the market that may significantly reduce the effect of collisions
andsometimesevenavoidthem(RefCitySafety).Inordertopreventanaccidentthereisaneedto
first assess and understand the traffic situation around the vehicle. The current state of the art
active safety systems utilize radars, cameras and laser based sensors to support environmental
sensing, i.e., to position the neighboring vehicles in relation to the host vehicle and the
infrastructure(roads,intersections,etc.).Futuresafetysystemswillobtainadditionalinformationby
communicating with other vehicles andtheinfrastructure,alsoby including new sensorslikea GPS
andamap.Suchinformationmaydramaticallyimprovetheaccuracyofourunderstandingthetraffic
situation,butasofnow,thedesignofsuchsystemshasnotbeenstudiedmuch.
This master thesis investigates how to combine information from several sensors including
GPS and internal vehicle sensors in order to position the ego vehicle and other objects around the
vehicle. Apart from designing the positioning systems, we also wish to identify difficulties and
understand different parts of the problem. These parts include sensor models, sensor fusion
algorithmsandtechniquestohandledatathatarriveswithatimedelay.

ii
ACKNOWLEDGMENTS
We would like to thank our examiner Lennart Svensson from Chalmers and our supervisor
HenrikLind from Volvo Car Corporation so much for bringing this interesting thesis work to us and
alsofortheircontinuoussupportthroughoutourwork.
We also thank Volvo Car Corporation and especially Mats Lestander for making the thesis
workpossiblewithprovidingeverythingweneededduringtheproject.

iii
TABLEOFCONTENTS
Abstract.........................................................................................................................................i
Acknowledgments........................................................................................................................ii
TableofContents........................................................................................................................iii
ListofAbbreviations.....................................................................................................................v
TableofFigures...........................................................................................................................vi
1 Introduction........................................................................................................................1
1.1 ThesisBackground..........................................................................................................1
1.2 AimoftheThesis.............................................................................................................2
1.3 Methodology...................................................................................................................2
1.4 DescriptionofTestEquipmentandConfigurations........................................................2
1.5 Limitations.......................................................................................................................4
1.6 OutlineoftheReport......................................................................................................5
2 TechnicalOverview.............................................................................................................6
2.1 PrinciplesofGPS.............................................................................................................6
2.1.1 GPSSignal................................................................................................................6
2.1.2 DeterminingReceiverPosition................................................................................7
2.1.3 GPSMessages.........................................................................................................8
2.1.4 GPSDataError......................................................................................................12
2.2 CoordinateSystems......................................................................................................12
2.2.1 GeographicalCoordinateSystem..........................................................................13
2.2.2 EarthCenteredEarthFixedCoordinateSystem...................................................13
2.2.3 MapProjection......................................................................................................14
2.2.4 EllipticParametersoftheEarthforGPSApplications..........................................15

iv
2.3 InternalVehicleSensors................................................................................................16
2.4 SensorFusionSystems..................................................................................................17
2.4.1 InertialNavigationSystem....................................................................................18
2.4.2 KalmanFilter.........................................................................................................18
3 MathematicalModelofAlgorithmsandConfiguration....................................................21
3.1 OverviewoftheAlgorithm............................................................................................21
3.1.1 SynchronizationofGPSandInternalSensors.......................................................22
3.1.2 ParametersoftheKalmanFilter...........................................................................23
3.1.3 DetectionofSignalQualitiesforGPSandInternalSensors..................................27
3.1.4 RelativeDistanceMeasurement...........................................................................28
4 Results...............................................................................................................................30
4.1 Testcase1..................................................................................................................30
4.2 Testcase2..................................................................................................................34
5 ConclusionandFutureWork............................................................................................43
Bibliography................................................................................................................................44
LISTOFABBREVIATIONS
CAN ControllerAreaNetwork
ECEF EarthCenteredEarthFixed
GPS GlobalPositioningSystem
HSCAN HighSpeedControllerAreaNetwork
Hz Hertz
IEEE InstituteofElectricalandElectronicsEngineers
INS InertialNavigationSystem
ITS IntelligentTransportationSystems
LPF LowPassFilter
NGA NationalGeospatialIntelligenceAgency
NIMA NationalImageryandMappingAgency
NMEA NationalMarineElectronicsAssociation
SNR SignaltoNoiseRatio
V2V VehicletoVehicle
WAVE WirelessAccessforVehicularEnvironments
WGS WorldGeodeticSystem
WLAN WirelessLocalAreaNetwork
latitude
longitude
h altitude
vi
TABLEOFFIGURES
Figure1.1Overviewofdeviceconnections..........................................................................................3
Figure2.1Calculationofsatellitedistance...........................................................................................7
Figure2.2VisualizationofGPSpositioning..........................................................................................8
Figure2.3ECEFandGeodeticcoordinates.........................................................................................14
Figure2.4Mercatorprojection...........................................................................................................15
Figure2.5Lowpassfilterappliedtoyawrate....................................................................................17
Figure2.6VisualizationoftheKalmangain........................................................................................19
Figure3.1Overviewofthesensorfusionsystem...............................................................................22
Figure3.2TheKalmanfilteralgorithm...............................................................................................25
Figure3.3Relativedistancemeasurement........................................................................................29
Figure4.1Testtrack1.........................................................................................................................30
Figure4.2Vehiclespeed.....................................................................................................................31
Figure4.3Lateraldistance..................................................................................................................31
Figure4.4Longitudinaldistance.........................................................................................................32
Figure4.5Targetangle.......................................................................................................................32
Figure4.6Lateralerrorhistogram......................................................................................................33
Figure4.7Longitudinalerrorhistogram.............................................................................................33
Figure4.8Targetangleerrorhistogram.............................................................................................34
Figure4.9Testtrack2.........................................................................................................................35
Figure4.10Vehiclespeed...................................................................................................................36
Figure4.11Lateraldistance................................................................................................................36
Figure4.12Longitudinaldistance.......................................................................................................37
Figure4.13Targetangle.....................................................................................................................37
Figure4.14Absolutedistanceerror...................................................................................................38
Figure4.15Lateralerrorhistogram....................................................................................................38
Figure4.16Longitudinalerrorhistogram...........................................................................................39
Figure4.17Targetangleerrorhistogram...........................................................................................39
Figure4.18GPSclockerror.................................................................................................................42

INTRODUCTION
In this section, the background of our thesis work will be introduced to the reader and the
goals will be defined. Technical equipments used in the project will also be briefly introduced. In
addition, the assumptions made, and the limitations put in the work will be presented. Finally, an
outlineofthereportisgivenattheendofthesection.
1.1 ThesisBackground
Todays active safety systems utilize radars, cameras and laser based sensors to support
environmental sensing, i.e., to position the neighboring vehicles in relation to the host vehicle and
the infrastructure (roads, intersections, etc.). Systems capability is dependent on the limits of the
sensorswhichallowtrackingofobjectsonlyinviewofthesensors.Futuresafetysystemswillobtain
additional information by communicating with other vehicles and the infrastructure, also by
includingnewsensorssuchasaGPS.Suchinformationmaydramaticallyimprovetheaccuracyofour
understanding the traffic situation and reduce possibility of accidents, but as of now, the design of
suchsystemshasnotbeenstudiedmuch.
In this thesis work relative distance calculation to support V2V communication is presented.
Considering the shortdistance between the cars driving on the road andusing similar GPSreceiver
products,it ispracticaltoassumethattheymayhavecommon errors(
c
)and also vehiclespecific
errors (
n
) in their individual position estimations. Therefore, we can define the equation of the
positionestimationforeachvehicleasfollows:
x
1
= x
1
+
1
+
c
x
2
= x
2
+
2
+
c
Ourexpectationwasalthougheachvehiclehaderroneouspositionestimationwiththemodel
definedabove;differentiationcoulddiscardthecommonerrorw
c
includedinbothvehiclepositions.
x
1
-x
2
= x
1
- x
2
+
1
-
2
To evaluate this idea, we logged and organized the necessary information and implemented a
system to make the best position estimations. In order to reduce the vehicle specific errors in the
calculations, other inputs (yaw rate, speed, gear, etc.) are also introduced to the system and
combined with a sensor fusion algorithm. There are different methods and algorithms such as
KalmanFiltering,BayesianNetworks,DempsterShaferTheoryandetc.usedtoimplementasensor

fusion system for different applications. A Kalman filter is used for improving position and thereby
distancemeasurementsinthismasterthesiswork.
1.2 AimoftheThesis
The target of this thesis work is to investigate the possible improvements in collision
avoidance systems that might become available with V2V communication. The main goals of our
workaredeterminingtherelativedistanceofatargetvehiclewithrespecttotheegovehicleasthey
share GPS and internal vehicle sensor information such as speed, yaw rate, steering angle and
analyzingtheaccuracy,dependenciesandchallengesofsuchasystem.
As of now, several associations and corporations are working on V2V communication
standards. One of them, IEEE, has recently developed the 802.11p standard, the socalled Wireless
AccessforVehicularEnvironments(WAVE)forIntelligentTransportationSystems(ITS),whichbrings
asetofadjustmentstothewellknown802.11WLANprotocol[1].However,duringthisthesiswork,
althoughthevehiclesdidnothavesuchbuiltincommunicationsystemphysically,weassumedthat
theyhadoneandtheycouldsharethenecessaryinformationwitheachother.
1.3 Methodology
Todesignanaccurateandefficientsensorfusionsystem,theprinciplesofGPSandthevehicle
sensors are studied first. Once the strengths and weaknesses of each of them are grasped, the
necessary sensors are chosen to be inputs to the system. The next thing is to choose the sensor
fusion algorithm and define the equations to combine the inputs in a reasonable way. After the
equations are defined and the sensor fusion algorithm is developed, CANalyzer software to log
information from CAN bus of the vehicle is installed and a C++ code is written to record GPS
information.Smalltestdataisloggedtoanalyzeandsolvesynchronizationproblemandonceallthe
information is synchronized, the sensor fusion algorithm is developed in MATLAB. When the
software and the hardware are prepared, test scenarios are created and thereby tests are done
accordingtothem.Finally,resultsofthetestsareanalyzedandpresented.
1.4 DescriptionofTestEquipmentandConfigurations
Testvehiclesthatwereusedinourprojectarestandard2007VolvoS80sedanand2008Volvo
V70 station wagon. Both vehicles are equipped with GlobalSat BU353 GPS receiver, standard
internalsensors(speed,yaw,gear)andonlyS80hadintegratedRadarCamerasensor.

GPSReceiver
GlobalSat BU353 GPS receiver was used in this thesis work. GPS receiver is equipped with a
high performance SiRF Star III GPS chipset, which outputs the information in NMEA 0183 format
throughits USB interface.We used GPRMC, GPGGAand GPGSVmessages of NMEA 0183 outputin
ourproject.
Table1SamplingfrequencyofGPSdata
Source SamplingTime SamplingFrequency
GPRMC GPS 1sec 1Hz
Latitude
Longitude
Speed
Time
GPGGA 1sec 1Hz
NumberofSatellites
GPGSV 5sec 0.2Hz
SNR
ControllerAreaNetwork(CAN)&VectorCANcaseXL
Controller area network (CAN) is a serial bus communication protocol suited for networking
sensors, actuators and other nodes in real time systems [2]. The protocol is widely used in the
automotive industry. CAN messages are kept in four different formats as data, remote, error and
overload. In our project, data messages are used. CAN message frame consists of Start of Frame
(SOF), Identifier, Remote Transmission Request (RTR), Control, Data, Cyclic Redundancy Checksum
(CRC),Acknowledgement(ACK)andEndofFrame(EOF).
Figure0.1Overviewofdeviceconnections
CANcaseXL CANalyzer
GPSLogger

SOF Identifier RTR Control Data CRC ACK EOF
Vector CANcaseXL is a USB interface, which is capable of processing CAN messages. It is

possibletoreceiveandanalyzeremoteframeswithoutanylimitation.Itisalsocapableofgenerating
and detecting error frames on the CAN bus [3]. CANcaseXL works with the Vector CANalyzer
software.
VectorCANalyzeris asoftware tool, which is used foranalyzingand observing ECU networks
anddistributedsystemsinvehicles.Inthisthesiswork,weusedCANalyzertoolinordertoobserve,
analyzeandrecorddatatrafficinCAN.
Weloggedspeed,gearandyawrateinformationfromHighSpeedControlAreaNetwork(HS
CAN).Thesamplingfrequenciesofthesesignalsareshowninthetablebelow.
Table2Samplingfrequencyofsensordata
Source SamplingTime SamplingFrequency
Speed HighSpeedCAN 20ms 50Hz
Gear 20ms 50Hz
Yaw 20ms 50Hz
Distance Radar 100ms 10Hz
Yaw 20ms 50Hz
1.5 Limitations
Inprincipal,thesystemthatisdevelopedinthisthesisworkreliesonaccuracyandefficiency
of the communication link between the vehicles. However, since we did not use an actual
communication system, we assumed existence of a link to share the information in between.
Therefore, the outcomes of this thesis work should be seen as the results obtained using a perfect
communicationsystemwithhighdatarate.
Anotherimportantassumptionistheabsoluteaccuracyofthereferencedata,withwhichwe
compareourtestresults.Weusedtheinformationfromtheradarenhancedwithacamerainorder
to obtain the true distance between the vehicles and compared our results with that information.
Consequently,theaccuracyofourtestresultsisrelativetotheaccuracyoftheradarinformation.

Moreover,sinceweusedtheradarcamerainformationasthereferenceandtheyrequireda
line of sight view within a narrow angle of the target vehicle for distance determination, we could
only test the cases where the vehicles were following each other or at least they drove in front of
eachother.
1.6 OutlineoftheReport
The report starts with the aim of this thesis work and the theoretical background of the
problem we are dealing with. The hardware and the software equipments used in this job are
introducedandthelimitationsaregiventoassistthereaderwhywehavemadecertaindecisionsin
thedesign.
Afterthisbriefintroduction,adetailedoverviewofGPSandvehiclesensorsincludingworking
principles,technicalspecifications,strengthsandweaknessesispresented.Theformalsensorfusion
algorithmandtheeffectsofitsparametersareexplained.
Inthethirdchapter,anoverviewoftheentirealgorithmincludinginputsofthesensorfusion
system,solutiontosynchronizationproblemandvehicledynamicsequationsisgiven.
Resultsobtainedfromthetestsareshownintheresultssection;thecommentsandpossible
workforthefuturearepresentedintheconclusion.

TECHNICALOVERVIEW
Inthissection,someimportantbackgroundinformationisgiventohelpthereaderfollowthe
restofthereporteasier.MainareasdiscussedareprinciplesoftheGlobalPositioningSystem(GPS),
coordinate systems, internal vehicle sensors and sensor fusion systems, particularly the Kalman
Filterwhichisusedinourproject.
1.7 PrinciplesofGPS
Global positioning system (GPS) is a satellite based positioning system. GPS satellites convey
informationdata,whichcanbeusedtocalculatethepositionofthereceiver.GPSincludes28active
satellites that are uniformly settled on six different circular orbits. Inclination angle of orbits from
the equator is 55 and orbits are seperated by 60 right accession from each other[4]. This
constellation provides global coverage and chance to operate 24 hours a day and in all weather
conditions. In theory three or more satellites in view is enough to calculate the location of the
receiver. Although receiver can calculate the position, the accuracy of positioning is dependent on
thenumberofsatellitesinview,signalqualities,weatherconditions,etc.
1.7.1 GPSSignal
EachGPSsatelliteconveysnavigationmessage,whichismadeoffivemajorcomponents[4].
1. SatelliteAlmanacData
Satellite Almanac Data includes orbital information for each satellite in the system. The
receiverusesAlmanacDatatocalculatetheapproximatelocationofeachsatelliteatanytime.
2. SatelliteEphemerisData
Satellite Ephemeris Data gives much more accurate information about satellite locations.
Ephemerisdataisconveyedfromaparticularsatelliteandit onlyincludeslocationof that satellite,
anditisvalidforonlyseveralhours[4].
3. SignalTimingData
GPS data includes time tags, which are used to calculate transmission time of specific points
on the GPS signal. This information is needed to determine the satellitetouser propagation delay
usedforranging[4].
4. IonosphericDelayData

Ionospheric delay data includes estimated amount of delay, when signal passes through
ionosphere.
5. SatelliteHealthMessage
Satellitehealthmessagecarriesinformationaboutsatelliteoperatingstatus.
1.7.2 DeterminingReceiverPosition
Each satellite transmits its exact orbital position and onboard clock time. The signal is
transmitted at speed of light (Suu - 1u
3
kmb). Time difference between the transmission of the
signalbythesatelliteandthereceivedtimebythereceiveriscalledtransittime.Distancebetween
thereceiverandthesatelliteisthencalculatedbymultiplyingthespeedoflightandthetransittime
().
J = - c
Figure0.1Calculationofsatellitedistance
The distance from a certain satellite provides a range circle, which indicates all the possible
positions of the receiver. Each satellite points a different range circle. In an ideal case, the range
circles are representedwith solidlinecircles shownin Figure0.2.Intersection of these circles gives
theestimatedreceiverposition.However,theserangecirclesaresensitivetoerrorsandconsidering
the fact that the signal propagates with speed of light, even small clock errors may cause large
positioningerrors.Forthisreason,theGPSreceiverclockandthesatelliteclockaresynchronizedto

provideaccuratepositioning.Althoughsynchronizedtimereducespositioningerrors,itdoesnotfix
theerrorscausedbyatmosphericconditionsandmultipatheffects.Theseerrorsaffectthetransmit
timeorthespeedofthesignalanditcreateslargerrangecircleswhichareshowninFigure0.2with
dashlines.
Figure0.2VisualizationofGPSpositioning
In theory, the receiver needs three satellite distance information to determine the position,
however, todays GPS receivers use information from at least four satellites to determine their
positions.Accuracyofpositioningdependsonthenumberofsatellitesinviewandthequalityofthe
receivedsignals.
1.7.3 GPSMessages
GPSreceiversprocessthemessagescomingfromGPSsatellites.Processedmessageisgivenas
NMEA,SiRF,Garmin,Delormeformatatoutputofthereceiver.MostoftheGPSreceiversuseNMEA

format, which is developed by National Marine Electronics Association. There are many kinds of
NMEAsentences.Someofthemarelistedbelow[5].
GPGGA Fixinformation
GPGLL Lat/Londata
GPGSA Overallsatellitedata
GPGSV Detailedsatellitedata
GPRMC RecommendedminimumdataforGPS
GPVTG Vectortrackanspeedovertheground
User needs at least one sentence from GPGGA, GPGGL or GPRMC in order to define the
positionofreceiver.InthismasterthesisreportGPGGA,GPGSVandGPRMCareused.
GPGGA
GPGGA packets enclose fix data, which gives three dimensional location of the receiver[6].
GPGGAsentencelookssimilarto:
$GPGGA,101428.000,5741.1742,N,01158.7346,E,1,10,0.8,46.9,M,40.1,M,,0000*69
<1>,<2>,<3>,<4>,<5>,<6>,<7>,<8>,<9>,<10>,<11>,<12>,<13>,<14>,<15>,<16>
ExplanationofaGPGGAsentenceisgivenbelow.
Field Example Comments

<1> SentenceID $GPGGA
<2> UTCTime 101428.000 hhmmss.sss
<3> Latitude 5741.1742 ddmm.mmmm
<4> N/SIndicator N N=North,S=South
<5> Longitude 01158.7346 dddmm.mmmm
<6> E/WIndicator E E=East,W=West
<7> PositionFix 1 0=Invalid,1=ValidGPSfix(SPS),
2=ValidDGPSfix,3=ValidPPSfix
<8> SatellitesUsed 10 Satellitesbeingused(012)
<9> HDOP 0.8 Horizontaldilutionofprecision
<10> Altitude 46.9 Altitude(WGS84ellipsoid)
<11> AltitudeUnits M M=Meters
<12> GeoidSeparation 40.1 Geoidseparation(WGS84ellipsoid)
<13> SeparationUnits M M=Meters

10
<14> TimesinceDGPS inseconds

<15> DGPSStationID
<16> Checksum *69 alwaysbeginwith*
GPRMC
GPRMC is a NMEA sentence, which keeps recommended minimum data for GPS [7]. It
includesposition,velocityandtimeinformation.GPRMCsentencelookssimilarto:
$GPRMC,101427.000,A,5741.1742,N,01158.7346,E,0.02,29.49,060810,,*35
<1>,<2>,<3>,<4>,<5>,<6>,<7>,<8>,<9>,<10>,<11>,<12>,<13>
ExplanationofaGPRMCsentenceisshownbelow.

<1> SentenceID $GPRMC
<2> UTCTime 101427.000 hhmmss.sss
<3> Status A A=Valid,V=Invalid
<4> Latitude 5741.1742 ddmm.mmmm
<5> N/SIndicator N N=North,S=South
<6> Longitude 01158.7346 dddmm.mmmm
<7> E/WIndicator E E=East,W=West
<8> Speedoverground 0.02 Knots
<9> Courseoverground 29.49 Degrees
<10> UTCDate 060810 DDMMYY
<11> Magneticvariation Degrees
<12> Magneticvariation E=East,W=West
<13> Checksum *35
GPGSV
GPGSV sentences provide elevation and azimuth angles of each satellite as well as signal to
noise ratio (SNR) of each signal [7]. GPGSV sentence can include maximum four satellites
information thus the number of GPGSV sentences depends on the number of satellite numbers in
view.Generally,threesentencesarenecessaryforfullinformation.GPGSVsentencelookssimilarto:
11
$GPGSV,3,1,12,27,72,271,41,15,55,191,38,09,55,275,40,17,38,102,42*7A
$GPGSV,3,2,12,26,33,159,38,28,31,059,28,18,27,276,19,22,21,313,33*72
$GPGSV,3,3,12,12,11,223,30,11,09,042,24,24,01,326,,08,01,094,*74
<1>,<2>,<3>,<4>,<5>,<6>,<7>,<8>,<9>,<10>,<11>,<12>,<13>,<14>,<15>,<16>
ExplanationofaGPRMCsentenceisshownbelow.

<1> SentenceID $GPGSV
<2> No.ofmessages 3 No.ofmessagesincomplete(13)
<3> Sequenceno. 1 Sequenceno.ofthisentry(13)
<4> Satellitesinview 12
<5> SatelliteID1 27 Rangeis132
<6> Elevation1 72 Elevationindegrees
<7> Azimuth1 271 Azimuthindegrees
<8> SNR1 41 SignaltonoiseratiodBHZ(099)
<9> SatelliteID2 15 Rangeis132
<10> Elevation2 55 Elevationindegrees
<11> Azimuth2 191 Azimuthindegrees
.
.
<> Checksum *70
ThetextbelowisapartofNMEA0183datareceivedfromtheGPSreceiverinoneofourtests
inGothenburg.
$GPRMC,101427.000,A,5741.1742,N,01158.7346,E,0.02,29.49,060810,,*35
$GPGGA,101428.000,5741.1742,N,01158.7346,E,1,10,0.8,46.9,M,40.1,M,,0000*69
$GPGSA,A,3,17,15,27,26,22,28,18,09,11,12,,,1.5,0.8,1.3*36
$GPGSV,3,1,12,27,72,271,41,15,55,191,38,09,55,275,40,17,38,102,42*7A
$GPGSV,3,2,12,26,33,159,38,28,31,059,28,18,27,276,19,22,21,313,33*72
$GPGSV,3,3,12,12,11,223,30,11,09,042,24,24,01,326,,08,01,094,*74

12
1.7.4 GPSDataError
Thereareseveralerrors,whicheffectandreduceaccuracyofGPS.Errorsaregroupedintofive
subgroups.Thesearebrieflyexplainedunderbelow:
1. IonosphericPropagationError
Ionosphere is the upper layer of the atmosphere. It consists of gases, which are ionized by
solar radiation. Ionized gases affect the speed of the GPS signal because signal cannot propagate
withfreespacepropagationspeedwhenitpassesthroughtheionosphere.Thisspeedchangecauses
modulationdelaysanderrorsinpseudorangemeasurement.
2. TroposphericPropagationError
Troposphereisthelowerlayeroftheatmosphere,whichiscomposedofwatervaporanddry
gases.Conditionintropospherelengthenspropagationpath,whichcausestroposphericpathdelay.
Tropospheric delay is not frequency dependent; thereby frequency dependent methods cannot
cancel tropospheric errors. Standard model of the atmosphere at the antenna is used to reduce
errorsthatarecausedbytroposphere[4].
3. MultipathEffectError
Objectsorsurfaces(ex:tallbuildings)aroundtheGPSreceivercanreflectoriginalGPSsignals.
Reflected signals can create new propagation paths and intersect with original GPS signals. This
reflections increase propagation time and makes distortion on the amplitude and the phase of
signal,therebycauseserrors.
4. EphemerisDataError
Each satellite conveys Ephemeris data, which gives information about the position of the
satellite. Difference between the computed satellite orbital position and the actual satellite orbital
position is called Ephemeris data error. Ephemeris data error is generally small and can be
eliminatedbyDGPS.
5. ReceiverClockError
A receiver's builtin clock is not as accurate as the atomic clocks onboard the GPS satellites.
Therefore,itmayhaveveryslighttimingerrors[8].
1.8 CoordinateSystems
Inpositioning,inordertopointanobjectonareferenceframe,acoordinatesystemmustbe
introduced. A coordinate system includes a set of numbers to define the location on the frame. By
usingthesenumbers,anypointintheframecanbemarkeduniquely.

13
1.8.1 GeographicalCoordinateSystem
The most common way of representing the locations on a spheroid is using geographical
coordinates.Ingeographicalcoordinates,therearethreeparametersdefiningtheexactlocationofa
pointontheEarth:latitude(),longitude()andaltitude(h).Formertwoareenoughtodetermine
the position on the surface of the Earth without the height information, which in most cases is
satisfactory. Latitude is the angle between the equatorial plane and the line, which is drawn from
the center of the Earth to the corresponding point on the surface, while longitude is the angle
between this line and the prime meridian crosssection. Altitude is the normal distance of a point
fromthesurfaceoftheEarth.
1.8.2 EarthCenteredEarthFixedCoordinateSystem
EarthCenteredEarthFixed(ECEF)coordinatesystemisaCartesiancoordinatesystem,which
is widely used in the GPS applications since it is considered to be a convenient way of locating a
point on the Earth [9]. The name of this system consists of two terms; EarthCentered and Earth
Fixed providing this convenience together. EarthCentered means that the origin of this coordinate
system is the center of the Earth, which makes anypoint to be located almost uniformly that way.
The latter provides the coordinates rotate together with the Earth, which lets them stay constant
despiteofthisphysicalrotation.Inthiscoordinatesystem,threeparametersX,YandZareusedto
determinethepositionontheEarthwhereXandYaxesappeartobeontheequatorialplaneandZ
axis lies perpendicular to them. According to this basis the coordinate set, (0, 0, 0) represents the
center of the Earth and (R, 0, 0) maps to the point where the prime meridian and the equator
intersect where R is the radius of the equator [10]. The relationship between the geodetic
coordinates and the ECEF coordinates is shown in Figure 0.3 and the mathematical conversions
betweeneachotherareshownbelow[11].
_ = (N +b) cos cos z z = orcton
= (N + b) cos sin z = orcton

z + ci
2
b sn
3
0
p - c
2
u cos
3
0
Z = [
b
2
u
2
N + b sin b =
p
cos0
- N
where;
N (roJius o cur:oturc) = o 1 -c
2
sin
2

14
o = scmi mo]or cortb oxis (cquotoriol roJius)

b = scmi minor cortb oxis (polor roJius)
p = X
2
+
2
0 = orcton
Zo
pb
Figure0.3ECEFandGeodeticcoordinates
1.8.3 MapProjection
TheprocessofrepresentingthesurfaceoftheEarthinatwodimensionalplaneiscalledmap
projection. There are many projection methods in use and in all these methods the surface is
deformed with certain rules and parameters. In this thesis work Mercator projection is used to
positiontheGPSreceiveronatwo dimensionalplaneandtheoptimizationalgorithmsaredoneon
thisplane.
Neicatoi piojection
Mercator projection is a conformal cylindrical projection presented by Gerardus Mercator in 1569,
which draws a set of horizontal lines representing the parallels and equally spaced vertical lines
corresponding to the meridians. Principle of the scaling in Mercator projection is shown in Figure
0.4.ConformalityprovidespreservingtheangleatanypointoftheEarthonthemap.Thisisoneof

15
the reasons that Mercator projection is widely used in navigation systems. However, while
preserving the accurate angle, the equal distances between the vertical lines cause the shape
distorted because the distance between the meridians actually varies depending on the latitude of
the point. The varying spaces between the horizontal lines provide the angles to be fixed with
respecttotheactualangleofadirectionwhilenothelpingthedistortedsizeortheshapeofthearea
tobecorrected[12].
Figure0.4Mercatorprojection
Themathematicalexpressionofthex,ycoordinatesofthemapisdeterminedby[13]
x = z - z
0
and
y = ln (tan() + sec ())
,
0
and are the longitude, the reference meridian and the latitude respectively. The first
equation provides equal distances between the vertical lines since x coordinates are calculated
regardless to the latitude information. The conformality is provided in the second equation by
scalingthedistancesalongthemeridianswithrespecttothelatitudes.
1.8.4 EllipticParametersoftheEarthforGPSApplications
The standard model used to determine the latitude, longitude and the altitude of a GPS
receiverisexplainedinadetailedwayinWorldGeodeticSystem1984(WGS84)byNationalImagery
and Mapping Agency (NIMA) which is known with the name National Geospatial Intelligence
Agency (NGA) currently [6]. The Earth is described as an ellipsoid in this model, which offers the

16
crosssections parallel to the equator being circular while the ones normal to the equator being
ellipsoid. The semimajor axis of the ellipsoid, which is normal to the equator and centered at the
centeroftheEarthisequaltotheradiusoftheequatorandistaken6,378.137km.Thesemiminor
axisofthisellipsoid,thesocalledpolarradiusoftheEarth,is6,356.7523142kmaccordingtoWGS
84.
1.9 InternalVehicleSensors
In vehicle positioning or relative distance measurement, using GPS is usually not enough to
get accurate results [14]. As mentioned in 1.7.4, GPS receivers suffer from many errors caused by
differentsources.Inordertoreducetheerrorsinthecalculations,otherinputsareintroducedtothe
system and combined with a sensor fusion algorithm. These inputs are the information received
from the internal sensors those expected to have a positive effect on the results. The sensors are
alreadysendinginformationtotherelevantpartsofthesysteminthevehiclethroughCANbusand
using this connection; the necessary information is forwarded to our sensor fusion system as well.
Thesensorsthoseareutilizedinthesystemarelistedanddescribedinthischapter.
1. VehicleSpeedoverGround
Vehicle speed over ground is reliable sensor information indicating the instant speed of the
vehicle.Itisusefulforestimatingthepositionofthevehicleorthedistancetoanobjectforafurther
timeinstance.
2. YawRate
Yaw rate is the information received from a sensor, which is able to determine the angular
velocity of the vehicle in degrees/second or radians/second. There are different kinds of sensors
capable of sensing angular velocity using different hardware and software. Yaw rate sensors are
usuallyconsideredtobetoonoisytherebynotveryreliablewhendrivinginlowspeeds.Inthatcase,
steering angle information becomes important to make a better estimation of the actual turning
angle. We also applied a lowpassfilter to reduce the noise from the yaw rate information (see
Figure0.5).

17
Figure0.5Lowpassfilterappliedtoyawrate
3. ReversedGear
Reversedgearistheindicatorforthegearposition,whichisimportanttodecidewhetherthe
vehicleismovingforwardorbackward.
Table3Propertiesofsensordata
Sensor information Data type Min Max Resolution Unit
Vehicle speed float 0 320 0.01 km/h
Yaw rate float -75 74.9998 0.03663 /sec
Gear level boolean 0 1
1.10 SensorFusionSystems
A sensor fusion system is a system that combines information from different sources to
achieve the least noisy result possible. Thereasonbehind the necessity of sensorfusionsystemsin

18
manyapplicationsisthefactthateachsensorhasitsownstrengthsandweaknesses,thereforeone
sensorisusuallynotreliableitself.
There are different methods and algorithms such as Kalman Filtering, Bayesian Networks,
DempsterShafer Theory etc. used to implement a sensor fusion system for different applications
[15]. The strengths and weaknesses of each input are introduced to these algorithms and the
reliability of each input is determined according to the working conditions and the output is
optimizedbythisway.Inthisthesiswork,aKalmanFilterisusedforimprovingpositionanddistance
measurements.
1.10.1 InertialNavigationSystem
Inertialnavigationsystemisanavigationthatcomputesthefurtherpositionofanobjectfrom
the given initial position using motion sensors such as speed and rotation according to the laws of
physics[16].Everynewpositioniscalculatedfromthepreviouslydeterminedpositionbyaprocess
known as dead reckoning. It is used in a wide range of applications including aircraft, spacecraft,
submarine,ship,mobilelandvehicleandhumannavigation.
Due to thenature of this systemthenewposition is always dependenton the previous one,
thereforetheerrorsincludedinthepreviouscalculationsarealsopreservedinthenewones.
1.10.2 KalmanFilter
One of the most wellknown methods for data/sensor fusion algorithms is the Kalman filter,
whichisnamedafterRudolfE.Kalmanwhoproposedalineardatafilteringalgorithminhisfamous
paper (Kalman 1960) in 1960 [17]. The problem to which the Kalman filter finds a solution is
estimatingtheoptimumstateofalineardynamicequationcorruptedbywhitenoiseusingrelevant
measurementsalsocontainingwhitenoise[18].Itisconsideredtobeoneofthebiggestdiscoveries
in statistical estimation theory and has been studied widely for various applications since its
discovery.
In principle, the Kalman filter has two distinguished steps consisting of prediction and
correction [17]. In prediction, the states of the dynamics equation are estimated with a prediction
noiseusingpredefineddynamicmodeloftheprocessusuallybeingalawofphysics.Thecorrection
stephasthemeasurementsofthesestateswithameasurementnoise.Predictionandmeasurement
noises are both normally distributed white noises and one should note that they are independent

19
from each other. The Kalman filter improves the predicted state using the measurement with a
weightedgain,whichiscalculatedaccordingtothenoisecovariancevalues[19].
Figure0.6VisualizationoftheKalmangain
The Kalman filtei algoiithm
TheKalmanfilterdoesnotrequirekeepingthehistoryofobservationsorestimationssinceit
isarecursivealgorithm,whichcalculatesthecurrentestimationonlyfromthelatestestimatedstate
and the current observation. As mentioned in the introduction of the Kalman filter 1.10.2, the
algorithm can be divided into two parts where in the first one, the socalled prediction step, the
stateisestimatedfromtheprevioustimeinstanceusingalinearpredictionequation;therebythea
priori estimate of the state at the current time instance is computed. The a priori estimate is
improvedwithanoisymeasurementfeedbackinthenextstepthatisknownasthecorrectionstep.
Thespecificformulasforthesestepsarepresentedbelow[17].
Prioristateestimate
Prioripredictionerrorcovariance
Residual
Residualcovariance
Kalmangain
Posterioristateestimate
Posterioripredictionerrorcovariance
where;
Aisthestatetransitionmodel,
Bisthecontrolinputmodel,
uisthecontrolvector,
Prediction
Correction

20
Qistheprocesserrorcovariance,
zisthemeasurement,
Histhemeasurementtransitionmodel,
Risthemeasurementerrorcovariance,
Iistheunitmatrix.
21
MATHEMATICALMODELOFALGORITHMSANDCONFIGURATION
In this section, the implementation details of our work will be presented with the
mathematical equations defined and the algorithm flowcharts. Furthermore, solutions to certain
problemssuchassynchronizationanddynamicsignalqualitydetectionarepresented.
1.11 OverviewoftheAlgorithm
The algorithms that we developed during this thesis work can be divided into three main
parts: information extraction, data synchronization and sensor fusion system. After the information
was simultaneously logged from the CAN bus and the GPS receivers in the two vehicles, first the
necessary data such as speed, yaw rate, latitude, longitude, SNR values and etc. were extracted by
the MATLAB codes we developed. However, neither in between the cars nor in the individual cars
amongtheGPSandthevehiclesensors,theinformationwassynchronized;therefore,thenextstep
wassynchronizingalltheinformation.Thedetailedexplanationofthewaywefollowedinthispartis
given in 1.11.1. Once the data is synchronized, two sensor fusion algorithms can work
simultaneously for the two cars and they can share the necessary information with each other
duringtheprocessshownbelow.

22
1.11.1 SynchronizationofGPSandInternalSensors
Inrealtimeapplications,dataorsignalsynchronizationisoneofthemostcriticalaspectsthat
should be designed and implemented carefully. In our case, although the tests were done offline,
thesensorfusionsystemrequiressynchronousinformationfromdifferentinputsinordertoprocess
the output correctly. Moreover, the outputs of the sensor fusion systems running in individual
vehiclesaresupposedtobesynchronouswitheachothertoo.
Figure0.1Overviewofthesensorfusionsystem
velocity
direction
velocity
x,y
x,y
direction
x, y
+
x,y
x,y
R
GPS
gear
gear
speed
speed
Q
Q
filtered
yawrate
filtered
yawrate
yaw
rate
yaw
rate
KalmanFilter
LPF
50
Hz
50
Hz
1Hz
KalmanFilter
LPF
50
Hz
GPS
R
50
Hz
1Hz
50
Hz
50
Hz

23
The timestamp information of the GPS messages was very sensitive, however there was no
exact match between the absolute time of the GPS and CAN messages. In order to calculate the
exacttimedifferencebetweenthem,wetookadvantageofthecrosscorrelationbetweenthespeed
signalsreceivedfrombothinputs.Thepositionofthemaximumcorrelationpointwithrespecttothe
referencepointgaveustheexacttimedifferencebetweenGPSandCAN;therefore,theinputsofthe
sensorfusionsystemweresynchronizedbyconsideringthisdifference.
AfterGPSandCANsignalsweresynchronized,thevehiclescouldbesynchronizedeasilyusing
thetimestampsofthesignalsfromtworeceiversthankstotheprecisetimeinformationintheGPS
messages.
1.11.2 ParametersoftheKalmanFilter
In this section, the actual parameters that were mapped into the formal Kalman filter
algorithmdescribedin1.10.2andtheinitializationofthestateswillbeexplained.
States anu the tiansition mouels of the Kalman filtei
The state matrix consists of only the position of the vehicle in our project; therefore, the matrixes
becomescalarvalues.ThestateandthemeasurementtransitionmodelsAandHarebothequalto
1.
x
k
-
= x
k-1
+t:
k
where
t is 20ms in our system and :
k
is the velocity of the vehicle at the precise moment. The detailed
explanation of the derived formulas determining the velocity of the vehicle is given below in this
section.
Theprioripredictionerrorcovariancebecomes
P
k
-
= P
k-1
+
k
sinceA = 1.
As explicitly shown in the previous formula, the process error covariance Q does not
necessarily stay constant throughout the algorithm; contrarily it is updated at every time sample k
byasubfunctionaccordingtothecharacteristicsoftheinternalvehiclesignalsthosetakepartinthe
prioriestimationstep.

24
Once the priori estimates are calculated, the correction step starts with calculating the
residual between the measured and the priori estimated position. One should notice that H was
takentobe1earlier.Thereforetheresidualandtheresidualcovarianceare
y
k
= z
k
- x
k
-
and
S
k
= P
k
-
+R
k
Again, the measurement error covariance R is updated for each new GPS signal by a sub
function,whichtakesseveralpropertiessuchasnumberofsatellitesusedinthepositioncalculation
andtheSignaltoNoiseRatio(SNR)ofthesignalsreceivedfromthesesatellitesintoaccount.
TheKalmangainisthencalculatedbythefollowingequation;
K
k
= P
k
-
S
k
-1
Theposterioristateandthepredictionerrorcovarianceestimationsbecome
x
k
= x
k
-
+ K
k
y
k
and
P
k
= (1 -K
k
)P
k
-
Initialization of the states

The Kalman filter recursively updates the states using the above mentioned formulas.
However, the states should be initialized before the first iteration in order to be updated in the
recursivealgorithm.
Theeasiestwayofinitializingthestatesisprobablysettingallto0sincetheywillbeupdated
laterintheprocessanyway.However,wechosetoinitializethepositionwiththefirstreceivedGPS
coordinates. On the other hand, the prediction error covariance P was set to 5 in the beginning,
whichbasicallymeansthatthefirstpositionisassumedtobeincludinganerrorwithavarianceof5
intheCartesiancoordinates.

25
NO YES
NO
NO YES
YES
Start
Read
GPSandSensor
Data
direction =NOTSET
Initializex,Q,P
Do
k=1to
lengthofdata
direction
==SET?
isGPS
available?
Isreadytoset
direction?
direction=SET
ComputeS,K,x,P
Computex,Q
x =x
Q=MAX
ComputeP
`
Computey,R
y =0
R =MAX
Figure0.2TheKalmanfilteralgorithm

26
Ineitial navigation foimulas foi piioii position estimation

Theprioripositionestimateofthevehiclewasgivenasthefollowingaboveinthissection.
x
k
-
= x
k-1
+t:
k
where
:
k
is the velocity of the vehicle. However, how the velocity is defined on a specific axis of the
Cartesiancoordinatesystemhasnotbeenintroducedyet.Obviouslyconsideringatwodimensional
Cartesiancoordinatesystem,thevelocityonthexoryaxisdependsonthedirectionofthevehicles
movement.In1.9,theinternalvehiclesensorsusedinthisthesisworkwerebrieflyexplainedandin
thissection,thespecificformulasusingtheinformationfromthesesensorswillbegiven.
Referringbackto1.9,yawratesignalgivesinformationontheangularvelocityofthevehicle,
which means how many degrees the heading direction of the vehicle will change in a second.
Accordingly, a 0 degrees/second of yaw rate means that the vehicle is heading straight, a 10
degrees/second and 5 degrees/second indicate that the vehicle will make an anticlockwise 10
degreesandaclockwise5degreeschangeinitsheadingdirectioninonesecondrespectively.Ifthe
current heading direction of the vehicle is known, it is always possible to determine the heading
usingtheyawrateinformationcontinuouslywiththefollowingequation.
J
k
= J
k-1
+ t
k-1
Once the direction of the vehicle is computed, the velocity on each axis on the coordinate
systemcanbecalculatedbycombiningthedirectionwiththespeedinformation.
:
x
k
= s
k
cos J
k
k
= s
k
sin J
k
Animportantpointinthevelocitycalculationsisthatitisafunction ofthedirectionandthe
direction is updated recursively by adding the yaw rate at each time, which causes the errors
included in the yaw rate to be added to the direction in a cumulative manner. After a while, the
direction gets highly corrupted because of this motive. In order to reduce this effect, we
implemented a sub function, which periodically keeps track of the two latest positions when the
observationsareavailableandcomputesthedirectionofthemovementusingasimplegeometrical
formulashownbelow.
J = tan
-1
_
y
2
-y
1
x
2
-x
1
]

27
1.11.3 DetectionofSignalQualitiesforGPSandInternalSensors
In1.10.2,itwasmentionedthatthelastdecisionontheoutputoftheKalmanfilterismadeby
the Kalman gain factor, which depends on the noise covariance of the prediction and the
observation. Therefore, the key point is introducing the covariance of these processes correctly to
theKalmanfilter.
Wealreadycitedin1.11.2thattheerrorcovariancevariesbytimeinbothsteps.Thenecessity
behind this variation is the fact that the conditions affecting the noise variance of these processes
actuallychangebytime.Forinstance,theobservationinoursystemisthepositioncalculatedbyGPS
receiverandaccuracyofthispositionmaychangeaccordingto thequalityofthesignalreceivedor
tothenumberofsatellitesthatwereusedincalculationsatthatmoment.Additionallythereliability
ofthesignalsreceivedfromthevehiclesensorsmayalsoalterbytheconditionsatthatmoment.
Starting with the quality of the prediction, which is inversely proportional to the noise
variance of the sensors used in our system, which are speed and yaw rate, after careful analysis of
the logged information from these sensors, we deduced that speed could be considered very
accurateallthetime,howeveryawratewasquitenoisywhendrivingatlowspeedsupto20km/h.
Sincetheprediction of the position is processingthe combined information from both sensors, the
noisyyawratedataaffectedtheresults.Therefore,thecovarianceofthepredictionprocessQwasa
functionofthespeedofthevehicle.
Ontheotherhand,itwasmorecomplicatedtodecideontheaccuracyoftheGPSpositiondue
to the fact that there were many things affecting the calculations such as different noise sources,
numberofvisiblesatellitesandSNRofthesignalsreceivedfromeachsatellite.
Wediscussedin1.7thattheremustbeatleastthreevisiblesatellitestobeabletodetermine
thepositionofaGPSreceiveronthesurfaceoftheEarth,andatleastfoursatellitestomanagethat
with the height information as well. Based on these facts, an idea for estimating the GPS location
accuracy is counting the number of satellite signal SNRs that are higher than a certain threshold.
Using different thresholds to notice the number of satellite signals above those levels was one
methodforunderstandingobservationerrorcovariance.
Another method we followed to determine the observation error covariance was calculating
the average SNR of all satellite signals that were used in the position calculations by the receiver.
Sincethefinalresultwasaffectedbyallthesignalsreceivedfromthosesatellites,itwaspracticalto
estimate the accuracy of the position using this average SNR. We implemented and tested both

28
methodsduringourworkandalthoughtheresultswereprettysimilar,wecametoaconclusionthat
usingaverageSNRprovidedslightlybetterresults.
However, we discovered that in certain cases, although the SNR values of the signals were
relatively high, the accuracy of the GPS position could be considerably poor when the vehicle was
driving around tall buildings or passing under obstacles such as bridges. Our guess is multipath
effectscausedtheresultstobecorruptedwithoutanysignificantdecreaseintheSNR.
1.11.4 RelativeDistanceMeasurement
Thefinalworktobedoneafterdeterminingthepositionsofthevehicleswasmeasurementof
the relative distance between them. As we mentioned in 1.8.3, Mercator projection was used for
positioningtheGPSreceiveronatwodimensionalplaneandtheoptimizationalgorithmsweredone
onthisplane.
The difference between the absolute positions on the universal plane only defines the
distance in northsouth and eastwest direction. However, in todays safety systems, each vehicle
needstodeterminethepositionsofthesurroundingvehicleswithrespecttoitsownheadingangle.
Therefore,eachvehicleneedstodefineanewcoordinatesystem,whichisshowninFigure0.3,sets
the vehicles heading angle as the primary horizontal axis indicated with x. Since the vehicle may
constantlychangeitsheadingangle,thecoordinatesystemisupdatedateverystepofthealgorithm
and the surrounding vehicles are positioned according to this dynamic coordinate frame
continuously.
The calculations that we made for the relative measurement at a certain time is given as
follows:
x
= x
2
-x
1
= y
2
- y
1
0 = tan
-1
(y x)
[ = -
x
i
= J - sin [
y
i
= J - cos [

29
Figure0.3Relativedistancemeasurement

30
RESULTS
In order to understand the accuracy, challenges and dependencies of a real time relative
distance measurement system; we did several tests in different areas (urban, suburban) and
environmentalconditions(weather,trafficconditions,etc.).Althoughresultsfromonlytwotestsare
covered comprehensively due to practical reasons, we nevertheless added a table of RMSE and
standarddeviationoftheerrorsforeachtestcaseattheendofthischapter.
1.12 Testcase1
ThisisoneoftheteststhatweredonearoundChalmersJohannebergcampusarea.Belowis
thedescriptionofthisparticulartestthatisstudied.
Figure0.1Testtrack1
Urbanarea
Mediumtraffic
Pursuit(3040meters)
Bothcarshavevaryingspeeds(1540km/h)
Weatherconditions:cloudyandrainy

31
Figure0.2Vehiclespeed
Figure0.3Lateraldistance

32
Figure0.4Longitudinaldistance
Figure0.5Targetangle

33
Figure0.6Lateralerrorhistogram
Figure0.7Longitudinalerrorhistogram

34
Figure0.8Targetangleerrorhistogram
1.13 Testcase2
The second test that will be covered in this section was done in a closed traffic path in
countryside.Thedescriptionofthetestisgivenbelow.
Countryside
Entryclosedtotraffic
Pursuit(2.4sec)
Bothcarshavevaryingspeeds(6080km/h)
Weatherconditions:clear
Path:mostlystraightloop

35
Figure0.9Testtrack2

36
Figure0.10Vehiclespeed
Figure0.11Lateraldistance

37
Figure0.12Longitudinaldistance
Figure0.13Targetangle

38
Figure0.14Absolutedistanceerror
Figure0.15Lateralerrorhistogram

39
Figure0.16Longitudinalerrorhistogram
Figure0.17Targetangleerrorhistogram

40
Table4RMSEandStandarddeviationoferrors
Case
Driving
Conditions
RMSElat(m)
Standard
Deviationlat
(m)
RMSElon(m)
Standard
Deviationlon
(m)
Case1
UrbanVarying
Speed
1.509937 1.463356 2.228135 1.801362
Case2
Countryside
Varyingspeed
1.369821 1.517508 2.030176 1.814933
Case3
Urban
VaryingSpeed
1.483793 1.225087 2.231224 1.803487
Case4
Countryside
Constantspeed
1.019622 0.879626 1.711952 1.317442
Case5
Countryside
Varyingspeed
1.365131 1.304811 2.314364 2.007950
Case6
Countryside
Varyingspeed
2.726428 2.671556 2.460274 2.222996
Case7
Countryside
Constantspeed
1.042331 0.892837 1.830348 1.426627
Case8
Countryside
Varyingspeed
1.631389 1.489401 2.376652 2.114173
Case9
Urban
VaryingSpeed
1.529207 1.439083 2.271184 1.940336
Case10
Countryside
Varyingspeed
1.462437 1.259776 2.189806 1.910531
Theresultsareanalyzedanddiscussedaccordingtothefiguresandthetableabove,however
one should consider that error analyses are done with the assumption of perfect accuracy of the
radarcamerainformationasmentionedbeforein1.5,whichisnottrueinreality.
We have mentioned before that due to the lack of true information on the absolute vehicle
positions;wewereonlyabletodotestswhentargetvehiclewasinthefieldofradarcameraview.
However, this was not the only problem caused by nonexistence of the true information. Another
thing was the difficultyof determining the errorcovariance of theGPSreceiverpositions. Since we
had no information on true positions, we could only use a limited number of experimental
informationatknownlocationsandonlyinimmobilesituations.

41
RMSerrorsandstandarddeviationoftheerrorsshowthatsensorfusionsystemwithGPSand
vehicle sensors could be a good way for relative distance measurement with around 1.5 and 2.2
meter average lateral and longitudinal RMSE respectively. By using more suitable models in the
sensorfusionsystemandrefiningthestatesofthefilter,theerrorscouldbeevenreduced.
First thing to be noticed from the lateral and longitudinal error histograms (see Figure 0.6,
Figure 0.7, Figure 0.15, Figure 0.16); their patterns do not have normal distribution. Main reason
behindthisistheinaccuratemodelsusedinthefusionsystem,whicharenotoptimalfornonlinear
systems.Thenonlinearityofthemotionandmeasurementmodelscausedtheerrordistributionto
reshape and have different characteristics. An Extended Kalman Filter, Unscented Kalman Filter or
ParticleFiltercouldprovidemuchbetterresults.
Another interesting point is that the mean of the longitudinal errors is not zero, which we
think was caused by the assumption of constant velocity during one cycle of the fusion system.
However, in reality, the speed of the vehicles had rapid changes at some points of the test drives,
which actually led to varying speeds during a fusion cycle. This behavior changed the mean of the
errorstononzerovalues.
As one of our goals was to understand the noise characteristics of GPS and errors
encounteredbythereceiver,wespentquitealotoftimetofindacorrelationbetweenSNRvalues
of the GPS signals and the positioning errors. However, we could neither end up with certain
thresholds nor could notice distinct behaviors in that sense. In some tests, while the SNR values
were relatively high, the accuracy could still be poor. Interestingly, the opposite scenario was
possibletoo.
42
Figure0.18GPSclockerror
Probably a more marked founding was the difference between two identical GPS receivers.
They could output very different SNR values at the same time, at the same place, which made the
situationevenmorecomplicated.Ontheotherhand,onecriticalGPSreceivererrorthatwenoticed
wastheclockerror.Inatestdrive,welocatedtwosameGPSproductsononevehicleandwantedto
analyze the behaviors of them. Unexpectedly, there was 1 second difference in the timestamps of
tworeceiversinoneparticulartest(seeFigure0.18).Consideringthefactthatweheavilyreliedon
the timestamp information of GPS, it is possible that we might have encountered the same
problems when they were positioned in different vehicles, which was impossible to notice in the
currentdesign.
Insummary,theresultsarenotasgoodasitcouldbebecauseofthesimplerdesignchoices,
butevenintheseconditionstheycouldbeconsideredencouraging.Bettersuitedmodelsandbetter
understandingoftheGPSerrordistributioncouldimprovetheresultssignificantly.
43
CONCLUSIONANDFUTUREWORK
Considering the importance of the safety systems in road traffic, collision avoidance systems
are most likely going to be improved continuously. Environmental awareness will always be one of
the key figures in these systems, therefore relative distance measurements will be an important
featureandmuchmorestudiesanddevelopmentsshouldbeexpected.
The results obtained from our studies showed that it is practical to share GPS and sensor
information between the vehicles to accomplish relative positioning. However; although relative
position estimations have reasonable accuracy, we are not sure how much of the error was
eliminated using the differential equation defined in 1.1, since we do not know the extent of the
errorintheabsolutepositionsduetothelackoftruepositioninformation.
First thing that might be refined to improve relative positions is the vehicle dynamics model
that is used in sensor fusion algorithm. There are already more accurate models used in cars for
differentapplicationssuchasstabilitycontrol,etc.Themodelusedinoursystemisignoringallthe
factorsthataffecttherawmovementsuchasweightandroadslip.
Secondly, the sensor fusion algorithm we used in the calculations is actually not the optimal
solution for nonlinear vehicle motion model and measurement model. The results could be
improvednotablywithamoresuitablealgorithm.
Weusedradarcamerainformationasreferencesincewedidnothaveanotheroption,butit
could actually be introduced to the sensor fusion system as well as with other potentially useful
sensors,sothatthecombinedresultscouldbeimprovedevenmore.
Finally, many more tests and analyses should be done to get a better understanding of the
noisesources,dependenciesoftheproblemandincreasetheaccuracyofresults.

44
BIBLIOGRAPHY
1.IEEE.IEEE802.11OfficialTimelines.[Online]
http://grouper.ieee.org/groups/802/11/Reports/802.11_Timelines.htm.
2.VehicleApplicationsofControllerAreaNetwork.Johanson,KarlHenrik,Trngren,Martin
andNielsen,Lars.
3.GmbH,VectorInformatic.VectorUserManual.2006.
4.MohinderS.Grewal,LawrenceR.Weill,AngusP.Andrews.GlobalPositioningSystems,
InertialNavigation,andIntegration.2001.
5.GpsInformationNMEA.[Online]http://gpsinformation.org/dale/nmea.htm#nmea.
6.NationalImageryandMappingAgency,DepartmentofDefence.WorldGeodeticSystem
1984(WGS84).2000.
7.TheGeodeticSurveySectionofSurveyandMappingOffice.[Online]
www.geodetic.gov.hk/smo/gsi/data/ppt/NMEAandRTCM.ppt.
8.StateWaterResourcesControlBoard.[Online]
http://www.swrcb.ca.gov/water_issues/programs/swamp/docs/cwt/guidance/6120.pdf.
9.Stovall,SherrylH.BasicInertialNavigation.s.l.:NavigationandDataLinkSectionSystem
IntegrationBranch,1997.
10.ElliottD.Kaplan,ChristopherJ.Hegarty.UnderstandingGPSPrinciplesandApplications
SecondEdition.2006.
11.UnderstandingCoordinateReferenceSystems,DatumsandTransformations.Janssen,V.
2009.
12.PennstateCollegeofEarthandMineralSciences.NatureofGeometricInformation.
[Online]https://www.eeducation.psu.edu/natureofgeoinfo/c2_p29.html.
13.Snyder,JohnP.MapProjectionsAWorkingManual.1987.
14.TohidArdeshiri,SogolKharrazi,JonasSjberg,JonasBrgman,MathiasLidberg.Sensor
FusionforVehiclePositioninginIntersectionActiveSafetyApplications.2006.
15.MartinE.Liggins,DavidL.Hall,JamesLlinas.HandbookofMultisensorDataFusion,
TheoryandPractice,SecondEdition.2009.
16.Woodman,OliverJ.Anintroductiontoinertialnavigation.2007.
17.GregWelch,GaryBishop.AnIntroductiontotheKalmanFilter.2001.

45
18.MohinderS.Grewal,AngusP.Andrews.KalmanFiltering:TheoryandPracticeUsing
MATLABSecondEdition.2001.
19.JamesL.Crawley,YvesDemazeau.PrinciplesandTechniquesforSensorDataFusion.
20.http://www.swrcb.ca.gov/water_issues/programs/swamp/docs/cwt/guidance/6120.pdf.
[Online]
46
APPENDIXA.SOURCECODES
A.1CANDataExtractorScript
%Thi s scr i pt ext r act s t he r el evant dat a f r omvehi cl e CAN bus
%i ncl udi ng i nt er nal sensor s and r adar / camer a f used dat a.

cl ose al l
cl ear al l
cl c

%Get t he l ocal t i me bef or e ext r act i on st ar t s
t 0 = cl ock;

%Set f i l e f ol der and f i l e name t o r ead
f i l eFol der = ' . . \ \ LogFi l es\ \ ' ;
f i l eName = ' Aug_20_CAN2_Case02. asc' ;
f ul l Fi l eName = [ f i l eFol der f i l eName] ;

%Read t he f i l e
CANFi l e = f open( f ul l Fi l eName, ' r t ' ) ;

%Request ed si gnal s
messageI D1 = ' 71B' ;
f uncHandl e1 = @ext r act _r adar _71B;

messageI D2 = ' 321' ;
f uncHandl e2 = @ext r act _hs_can_speed;

%messageI D3 = ' 83' ;
%f uncHandl e3 = @ext r act _hs_can_gear ;

%messageI D4 = ' 94' ;
%f uncHandl e4 = @ext r act _hs_can_st eer i ng;

f uncHandl e5 = @ext r act _hs_can_yaw;

f uncHandl e6 = @ext r act _r adar _yaw;

messageI D7 = ' 71C' ;
f uncHandl e7 = @ext r act _r adar _71C;

f uncHandl e8 = @ext r act _hs_can_r evgear ;

%messageI D9 = ' 321' ;
%f uncHandl e9 = @ext r act _hs_can_accel er at i on;

%I ni t i al i ze i ndexes f or each vect or

47
i ndex1 = 1;
i ndex2 = 1;
%i ndex3 = 1;
%i ndex4 = 1;
i ndex5 = 1;
i ndex6 = 1;
i ndex7 = 1;
i ndex8 = 1;
%i ndex9 = 1;

%Maxi mumDLC val ue
maxDLC = 8;

%Li ne count er i n t he ent i r e CAN l og
l i neCount er = 0;

i ni t i al Capaci t y = 1000;
capaci t yExt ensi on = 1000;

capaci t y1 = i ni t i al Capaci t y;
%capaci t y3 = i ni t i al Capaci t y;

Can. r adar 71B = i ni t i al i zeRadar 71B( capaci t y1) ;
Can. hsSpeedOver Gr ound = zer os( 1, capaci t y2) ;
%Can. hsGear Lever Posi t i on = zer os( 1, capaci t y3) ;

%Can. hsSt eer i ngAngl eSi gn = zer os( 1, capaci t y4) ;
%Can. hsSt eer i ngAngl e = zer os( 1, capaci t y4) ;

Can. hsYaw = zer os( 1, capaci t y5) ;
Can. r adar Yaw = zer os( 1, capaci t y6) ;
Can. r adar 71C = i ni t i al i zeRadar 71C( capaci t y7) ;
Can. hsRevGear = zer os( 1, capaci t y8) ;
%Can. hsAccel er at i on = zer os( 1, capaci t y9) ;

whi l e ~f eof ( CANFi l e)

%Read t he next l i ne f r omt he f i l e
t hi sLi ne = f get l ( CANFi l e) ;

% %I ncr ement t he l i ne count er
% l i neCount er = l i neCount er + 1;
% di sp( spr i nt f ( ' Li ne: %u' , l i neCount er ) ) ;

48
[ t empToken, t empSt r i ng] = st r t ok( t hi sLi ne) ;

t empTi me = st r 2doubl e( t empToken) ;

i f t empTi me >= 0
[ t empToken, t empSt r i ng] = st r t ok( t empSt r i ng) ;

t empChannel = st r 2num( t empToken) ;

i f ~i sempt y( t empChannel ) && i shex( st r t ok( t empSt r i ng) )

[ t empMessageI D, t empSt r i ng] = st r t ok( t empSt r i ng) ;

%Get dat a byt es f r omt he cur r ent l i ne
Dat aByt es = get Dat aByt es( t hi sLi ne) ;

%I s i t Message 1?
i f st r cmp( t empMessageI D, messageI D1)
i f i ndex1 == capaci t y1
Can. r adar 71B = ext endRadar 71B( Can. r adar 71B, . . .
capaci t yExt ensi on) ;
capaci t y1 = capaci t y1 + capaci t yExt ensi on;
end
[ Can. r adar 71B. t ar get RangeAccel er at i on( i ndex1) , . . .
Can. r adar 71B. t ar get RangeRat e( i ndex1) , . . .
Can. r adar 71B. det ect i onSensor ( i ndex1) , . . .
Can. r adar 71B. det ect i onSt at us( i ndex1) , . . .
Can. r adar 71B. t ar get Range( i ndex1) , . . .
Can. r adar 71B. t ar get Mot i onCl ass( i ndex1) , . . .
Can. r adar 71B. t ar get I d( i ndex1) , . . .
Can. r adar 71B. t ar get Wi dt h( i ndex1) , . . .
Can. r adar 71B. t ar get Moveabl eSt at us( i ndex1) , . . .
Can. r adar 71B. t ar get Angl e( i ndex1) ] . . .
= f uncHandl e1( Dat aByt es) ;

i ndex1 = i ndex1 + 1;

%I s i t Message 2?
el sei f st r cmp( t empMessageI D, messageI D2)
Can. hsSpeedOver Gr ound = [ Can. hsSpeedOver Gr ound . . .
zer os( 1, capaci t yExt ensi on) ] ;
end
Can. hsSpeedOver Gr ound( i ndex2) = f uncHandl e2( Dat aByt es) ;

% %I s i t Message 3?
% el sei f st r cmp( t empMessageI D, messageI D3)
% i f i ndex3 == capaci t y3
% Can. hsGear Lever Posi t i on = [ Can. hsGear Lever Posi t i on . . .
% zer os( 1, capaci t yExt ensi on) ] ;
% capaci t y3 = capaci t y3 + capaci t yExt ensi on;
% end
% Can. hsGear Lever Posi t i on( i ndex3) = f uncHandl e3( Dat aByt es) ;

49
% i ndex3 = i ndex3 + 1;

%I s i t Message 4?
% Can. hsSt eer i ngAngl eSi gn = [ Can. hsSt eer i ngAngl eSi gn . . .
% Can. hsSt eer i ngAngl e = [ Can. hsSt eer i ngAngl e . . .
% end
% [ Can. hsSt eer i ngAngl eSi gn( i ndex4) , . . .
% Can. hsSt eer i ngAngl e( i ndex4) ] = f uncHandl e4( Dat aByt es) ;

%I s i t Message 5?
Can. hsYaw = [ Can. hsYaw . . .
end
Can. hsYaw( i ndex5) = f uncHandl e5( Dat aByt es) ;

%I s i t Message 6?
Can. r adar Yaw = [ Can. r adar Yaw . . .
end
Can. r adar Yaw( i ndex6) = f uncHandl e6( Dat aByt es) ;

%I s i t Message 7?
Can. r adar 71C = ext endRadar 71C( Can. r adar 71C, . . .
capaci t yExt ensi on) ;
end
[ Can. r adar 71C. t ar get RangeAccel er at i on( i ndex7) , . . .
Can. r adar 71C. t ar get RangeRat e( i ndex7) , . . .
Can. r adar 71C. det ect i onSensor ( i ndex7) , . . .
Can. r adar 71C. det ect i onSt at us( i ndex7) , . . .
Can. r adar 71C. t ar get Range( i ndex7) , . . .
Can. r adar 71C. t ar get Mot i onCl ass( i ndex7) , . . .
Can. r adar 71C. t ar get I d( i ndex7) , . . .
Can. r adar 71C. t ar get Wi dt h( i ndex7) , . . .
Can. r adar 71C. t ar get Moveabl eSt at us( i ndex7) , . . .
Can. r adar 71C. t ar get Angl e( i ndex7) ] . . .
= f uncHandl e7( Dat aByt es) ;


50

%I s i t Message 8?
Can. hsRevGear = [ Can. hsRevGear . . .
end
Can. hsRevGear ( i ndex8) = f uncHandl e8( Dat aByt es) ;

% %I s i t Message 9?
% Can. hsAccel er at i on = [ Can. hsAccel er at i on . . .
% end
% Can. hsAccel er at i on( i ndex9) = f uncHandl e9( Dat aByt es) ;
end
end
end
end

Can. r adar 71B = r emoveAppendedFr omRadar 71B( Can. r adar 71B, i ndex1 - 1) ;
Can. hsSpeedOver Gr ound = Can. hsSpeedOver Gr ound( 1 : i ndex2 - 1) ;
%Can. hsGear Lever Posi t i on = Can. hsGear Lever Posi t i on( 1 : i ndex3 - 1) ;

%Can. hsSt eer i ngAngl eSi gn = Can. hsSt eer i ngAngl eSi gn( 1 : 2 : i ndex4 - 1) ;
%Can. hsSt eer i ngAngl e = Can. hsSt eer i ngAngl e( 1 : 2 : i ndex4 - 1) ;

Can. hsYaw = Can. hsYaw( 1 : i ndex5 - 1) ;
Can. r adar Yaw = Can. r adar Yaw( 1 : i ndex6 - 1) ;

Can. r adar 71C = r emoveAppendedFr omRadar 71C( Can. r adar 71C, i ndex7 - 1) ;
Can. hsRevGear = Can. hsRevGear ( 1 : i ndex8 - 1) ;
%Can. hsAccel er at i on = Can. hsAccel er at i on( 1 : i ndex9 - 1) ;

%Cl ose t he f i l e
f cl ose( CANFi l e) ;

%Set t he name of t he bi nar y f i l e t o save var i abl es
mat Fi l eName = [ ' . . \ \ Resour ces\ \ ' f i l eName( 1: end- 3) ' mat ' ] ;

%Save t he var i abl es
save( mat Fi l eName, ' Can' ) ;

%Cal cul at e el apsed t i me f or al l pr ocess
el apsedTi me = et i me( cl ock, t 0) ;
el apsedMi nut es = f l oor ( f l oor ( el apsedTi me) / 60) ;

51
el apsedSeconds = el apsedTi me - ( el apsedMi nut es * 60) ;

di sp( spr i nt f ( ' El apsed t i me f or t he oper at i ons: %2u mi n %5. 2f sec\ n' , . . .
el apsedMi nut es, el apsedSeconds) ) ;
A.2GPSDataExtractorScript
%Thi s scr i pt ext r act s t he r el evant dat a f r omGPS l ogs.

cl ose al l
cl ear al l
cl c

f or gpsNo = 1: 1
f or caseNo = 2: 3

%Set GPS f i l e f ol der and f i l e name
f i l eFol der = ' . . \ \ LogFi l es\ \ ' ;
f i l eName = spr i nt f ( ' Aug_20_GPS%d_Case%02d. t xt ' , gpsNo, caseNo) ;
f ul l Fi l eName = [ f i l eFol der f i l eName] ;

%Read GPS f i l e
gpsFi l e = f open( f ul l Fi l eName, ' r t ' ) ;

%I ni t i al i ze t he i ndex f or each di f f er ent message
i ndexGPRMC = 1;
i ndexGPGGA = 1;
i ndexGPGSA = 1;
i ndexGPGSV = 1;

%I ni t i al i ze GPGSV St r uct f or f i r st t i me cal l t o t he f unct i on
Gps. gpgsv. el evat i on = 0;
Gps. gpgsv. azi mut h = 0;
Gps. gpgsv. snr = 0;
Gps. gpgsv. sat Pr nNo = 0;

%Fl ag I dent i f i er s f or GPGSV message
LAST_MESSAGE_GPGSV = 1;
NOT_LAST_MESSAGE_GPGSV = 0;

%I ndex of t he message I Ds
messageI dI ndex = 1: 6;

%Ext r act GPS I nf or mat i on unt i l t he end of t he f i l e
whi l e ~f eof ( gpsFi l e)

%Read t he next l i ne
gpsl i ne = f get s( gpsFi l e) ;
i f ~i sempt y( f i nd( gpsl i ne == ' *' , 1) )

52
%I s i t GPRMC?
i f st r cmp( ' $GPRMC' , gpsl i ne( messageI dI ndex) )

%Ext r act GPRMC message
[ Gps. gpr mc. t ype( i ndexGPRMC, : ) , . . .
Gps. gpr mc. t i me( i ndexGPRMC, : ) , . . .
Gps. gpr mc. st at us( i ndexGPRMC, : ) , . . .
Gps. gpr mc. l at i t ude( i ndexGPRMC ) , . . .
Gps. gpr mc. l ongi t ude( i ndexGPRMC ) , . . .
Gps. gpr mc. speed( i ndexGPRMC ) , . . .
Gps. gpr mc. angl e( i ndexGPRMC ) , . . .
Gps. gpr mc. dat e( i ndexGPRMC, : ) ] . . .
= ext r act _GPS_gpr mc( gpsl i ne) ;

i ndexGPRMC = i ndexGPRMC + 1;

%I s i t GPGGA?
el sei f st r cmp( ' $GPGGA' , gpsl i ne( messageI dI ndex) )

%Ext r act GPGGA message
[ Gps. gpgga. t ype( i ndexGPGGA, : ) , . . .
Gps. gpgga. t i me( i ndexGPGGA, : ) , . . .
Gps. gpgga. l at i t ude( i ndexGPGGA ) , . . .
Gps. gpgga. l ongi t ude( i ndexGPGGA ) , . . .
Gps. gpgga. f i xQual i t y( i ndexGPGGA ) , . . .
Gps. gpgga. noOf Sat ( i ndexGPGGA ) ] . . .
= ext r act _GPS_gpgga( gpsl i ne) ;

i ndexGPGGA = i ndexGPGGA + 1;

% %I s i t GPGSA?
% el sei f st r cmp( ' $GPGSA' , gpsl i ne( messageI dI ndex) )
%
% %Ext r act GPGSA message
% Gps. gpgsa( i ndexGPGSA) = ext r act _GPS_gpgsa( gpsl i ne) ;
% i ndexGPGSA = i ndexGPGSA + 1;

%I s i t GPGSV?
el sei f st r cmp( ' $GPGSV' , gpsl i ne( messageI dI ndex) )

%Ext r act GPGSV message
[ Gps. gpgsv, f l ag] = ext r act _GPS_gpgsv( gpsl i ne, . . .
Gps. gpgsv, . . .
i ndexGPGSV) ;

%Was i t t he l ast message?
i f f l ag == LAST_MESSAGE_GPGSV

%I ncr ease t he i ndex
i ndexGPGSV = i ndexGPGSV + 1;
end
end
end
end

53

%Cl ose t he f i l e
f cl ose( gpsFi l e) ;

%Set t he name of t he bi nar y f i l e t o save var i abl es
mat Fi l eName = [ ' . . \ \ Resour ces\ \ ' f i l eName( 1: end- 3) ' mat ' ] ;

%Save t he var i abl es
save( mat Fi l eName, ' Gps' ) ;

cl ear Gps

end
end
A.3SynchronizationofGPSandCAN
f unct i on [ Can, Gps] = synchr oni zeCanGps( Can, Gps)
%Thi s f unct i on synchr oni zes t he CAN and GPS messages by appl yi ng cr oss
%cor r el at i on on t he speed si gnal s r ecei ved f r ombot h i nput s.

%Lengt h det er mi nat i on
l engt hCan_ = l engt h( Can. hsSpeedOver Gr ound) ;
l engt hGps_ = l engt h( Gps. gpr mc. speed) ;

%Synchr oni zat i on set up
est i mat edPoi nt _ = max( l engt hGps_, l engt hCan_) ;
cor r Resul t _ = xcor r ( Gps. gpr mc. speed, Can. hsSpeedOver Gr ound) ;
[ maxVal _ maxPoi nt _] = max( cor r Resul t _) ;
shi f t i ng_ = est i mat edPoi nt _ - maxPoi nt _;

%Synchr oni zat i on of Gps and Can i nf or mat i on
i f shi f t i ng_ < 0
Gps = r emoveFi r st NFr omGps( Gps, - shi f t i ng_) ;
el se
Can = r emoveFi r st NFr omCan( Can, shi f t i ng_) ;
end

r et ur n
A.4MercatorProjection
f unct i on [ x_, y_] = get Mer cat or ( l at _, l on_)
%Thi s f unct i on cal cul at es t he mer cat or pr oj ect i on f r omt he gi ven
%geogr aphi c coor di nat es.

x_ = l on_;
y_ = r ad2deg( l og( t and( l at _) + secd( l at _) ) ) ;

r et ur n

54
A.5HeadingUpdatetoRemoveBiasinYawRate
f unct i on headi ng_ = updat eHeadi ng( Can_, Y_, X_, k_)
%Thi s f unct i on updat es t he headi ng of a car usi ng pr evi ous posi t i ons.
%Funct i on shoul d be cal l ed per i odi cal l y t o r educe t he ef f ect s of yaw bi as.

%Get di st ance i n x and y coor di nat es bet ween t wo pr evi ous consecut i ve
%posi t i ons when GPS si gnal r ecei ved.
y = Y_( k_ - 50) - Y_( k_ - 100) ;
x = X_( k_ - 50) - X_( k_ - 100) ;

%Cal cul at e headi ng of t he vehi cl e
headi ng_ = at and( y / x) ;

%I f t he angl e i s supposed t o be i n t he 2nd or 3r d zone of t he
%Car t esi an coor di nat e syst em?
i f x < 0
headi ng_ = headi ng_ - 180;
end

%Add t he headi ng change f r omt he l ast t i me i nst ant t i l l now.
headi ng_ = mod( headi ng_ + sum( 20e- 3 . * Can_. hsYaw( k_- 49: k_) ) , 360) ;

r et ur n
A.6MainScriptforSensorFusion
cl ose al l
cl ear al l
cl c

%Case det ai l s
mont h = ' Aug' ;
day = 6;
caseNo = 11;

%Fol der t o access l ogged i nf or mat i on
f i l eFol der = ' . . \ Resour ces\ ' ;

%Load synchr oni zed GPS and CAN dat a f r ombot h vehi cl es
l oad( [ f i l eFol der spr i nt f ( ' %s_%02d_GPS1_Case%02d_N. mat ' , mont h, day,
caseNo) ] ) ;
l oad( [ f i l eFol der spr i nt f ( ' %s_%02d_CAN1_Case%02d_N. mat ' , mont h, day,
caseNo) ] ) ;
l oad( [ f i l eFol der spr i nt f ( ' %s_%02d_GPS2_Case%02d_N. mat ' , mont h, day,
caseNo) ] ) ;
l oad( [ f i l eFol der spr i nt f ( ' %s_%02d_CAN2_Case%02d_N. mat ' , mont h, day,
caseNo) ] ) ;

%Common l engt h of t he ar r ays
l en = l engt h( Can1. hsYaw) ;

55
%Copy t he l at i t ude and l ongi t ude i nf or mat i on t o ar r ays

l at 1Gps = Gps1. gpgga. l at i t ude;
l on1Gps = Gps1. gpgga. l ongi t ude;
l at 2Gps = Gps2. gpr mc. l at i t ude;
l on2Gps = Gps2. gpr mc. l ongi t ude;

%St at i c memor y al l ocat i on f or t he ar r ays

%Pr i or i st at es
Lat 1Hat = zer os( 1, l en) ;
Lon1Hat = zer os( 1, l en) ;
Lat 2Hat = zer os( 1, l en) ;
Lon2Hat = zer os( 1, l en) ;

%Post er i or i st at es
Lat 1Opt = zer os( 1, l en) ;
Lon1Opt = zer os( 1, l en) ;
Lat 2Opt = zer os( 1, l en) ;
Lon2Opt = zer os( 1, l en) ;

Y1Opt = zer os( 1, l en) ;
X1Opt = zer os( 1, l en) ;
Y2Opt = zer os( 1, l en) ;
X2Opt = zer os( 1, l en) ;

%Pr e- def i ned er r or covar i ance val ues f or di f f er ent r el i abi l i t y l evel s
FULL_TRUST = 2 * 10e- 5;
HI GH_TRUST = 5 * 10e- 5;
HALF_TRUST = 10 * 10e- 5;
LOW_TRUST = 15 * 10e- 5;
NO_TRUST = 30 * 10e- 5;

%Pr edi ct i on pr ocesses er r or covar i ances
P1 = HALF_TRUST;
P2 = HALF_TRUST;

%I ni t i al i ze t he di r ect i on of t he car s
% 0 - > East
% 90 - > Nor t h
%180 - > West
%- 90 - > Sout h
di r ect i on1 = 0; %Case 1 - > - 1
di r ect i on2 = 0; %Case 1 - > 20

%Pr epr ocessor const ant s f or di r ect i on set
SET = 1;
NOT_SET = 0;

%Reset di r ect i on set f l ags f or t he car s
di r ect i onFl ag1 = NOT_SET;
di r ect i onFl ag2 = NOT_SET;

56
%St ar t sensor f usi on

f or k = 1: l en

%I f t he di r ect i on of t he f i r st car det er mi ned?
i f di r ect i onFl ag1 == SET

%Est i mat e t he posi t i on of t he f i r st vehi cl e f or t he cur r ent t i me
%( Pr i or i est i mat i on)
di r ect i on1 = get Di r ect i on( Can1. hsYaw( k) , di r ect i on1) ;

%Cal cul at e vel oci t y on t he l at i t ude and t he l ongi t ude
xVel oci t y1 = cal cul at eXVel oci t y( di r ect i on1,
Can1. hsSpeedOver Gr ound( k) , Can1. hsRevGear ( k) ) ;
yVel oci t y1 = cal cul at eYVel oci t y( di r ect i on1,

%Cal cul at e a pr i or i l at i t ude and l ongi t ude est i mat es
[ Lat 1Hat ( k) , Lon1Hat ( k) ] = updat eLat Lon( Lat 1Opt ( k- 1) , Lon1Opt ( k- 1) ,
xVel oci t y1, yVel oci t y1) ;

%Det er mi ne sensor and GPS er r or covar i ances
Q1 = set Sensor Qual i t y( Can1, k) ;
R1 = set GpsQual i t y( Gps1, Can1, k) ;

%Comput e pr ocess er r or covar i ance
P1 = P1 + Q1;

%I f GPS i s not avai l abl e or not r el i abl e at al l
i f mod( k, 50) ~= 1 | | R1 == NO_TRUST

%Then, set r esi dual = 0
y1Lat = 0;
y1Lon = 0;

%I f GPS i s avai l abl e and at l east somewhat r el i abl e
el se
% di sp( spr i nt f ( ' R1 = %f ' , R1) ) ;

%Comput e r esi dual bet ween obser vat i on and pr edi ct i on
y1Lat = l at 1Gps( k) - Lat 1Hat ( k) ;
y1Lon = l on1Gps( k) - Lon1Hat ( k) ;

%Updat e t he di r ect i on per i odi cal l y ( ever y 5 seconds) i f t he
%r equi r ement s ar e f ul f i l l ed
i f k > 250 && mod( k, 250) == 2 && . . .
i sempt y( f i nd( abs( Can1. hsYaw( k- 100: k- 1) ) > 2) ) &&
Can1. hsSpeedOver Gr ound( k- 100) > 15 && Can1. hsSpeedOver Gr ound( k- 50) > 15

di sp( spr i nt f ( ' V1 - > k = %d' , k) ) ;
di sp( spr i nt f ( ' Pr evi ous di r ect i on = %f ' , di r ect i on1) ) ;
di r ect i on1 = updat eDi r ect i on( Can1, Y1Opt , X1Opt , k) ;

57
di sp( spr i nt f ( ' Updat ed di r ect i on = %f \ n' , di r ect i on1) ) ;

end

end

%Updat e measur ement er r or covar i ance
S1 = P1 + R1;

%Cal cul at e Kal man Gai n
K1 = P1 / S1;

%Cor r ect t he est i mat ed posi t i on
%( Post er i or i est i mat i on)
Lat 1Opt ( k) = Lat 1Hat ( k) + K1 * y1Lat ;
Lon1Opt ( k) = Lon1Hat ( k) + K1 * y1Lon;

%Updat e t he er r or covar i ance
P1 = ( 1 - K1) * P1;

%I f t he di r ect i on i s not det er mi ned yet ?
el se

% di sp( spr i nt f ( ' V1 - > Fl ag Not Set ! k = %d' , k) ) ;
%Post er i or i est i mat i on i s equal t o t he measur ement
Lat 1Opt ( k) = l at 1Gps( k) ;
Lon1Opt ( k) = l on1Gps( k) ;

%Check i f t he di r ect i on can be det er mi ned now?
i f k > 4*50 && i sReadyToSet Di r ect i on( Can1, Gps1, k)
di r ect i on1 = set Di r ect i on( Gps1, k) ;
di r ect i onFl ag1 = SET;
end

end

%Get Mer cat or pr oj ect ed coor di nat es
[ X1Opt ( k) , Y1Opt ( k) ] = get Mer cat or ( Lat 1Opt ( k) , Lon1Opt ( k) ) ;

%I f t he di r ect i on of t he f i r st car det er mi ned?
i f di r ect i onFl ag2 == SET

%Est i mat e t he posi t i on of t he second vehi cl e f or t he cur r ent t i me
di r ect i on2 = get Di r ect i on( Can2. hsYaw( k) , di r ect i on2) ;

%Cal cul at e vel oci t y on t he l at i t ude and t he l ongi t ude
xVel oci t y2 = cal cul at eXVel oci t y( di r ect i on2,
yVel oci t y2 = cal cul at eYVel oci t y( di r ect i on2,

%Cal cul at e a pr i or i l at i t ude and l ongi t ude est i mat es

58
[ Lat 2Hat ( k) , Lon2Hat ( k) ] = updat eLat Lon( Lat 2Opt ( k- 1) , Lon2Opt ( k- 1) ,

xVel oci t y2, yVel oci t y2) ;

%Det er mi ne sensor and GPS er r or covar i ances
Q2 = set Sensor Qual i t y( Can2, k) ;
R2 = set GpsQual i t y( Gps2, Can2, k) ;

%Comput e pr ocess er r or covar i ance
P2 = P2 + Q2;

%I f GPS i s not r el i abl e at al l ?
i f mod( k, 50) ~= 1 | | R2 == NO_TRUST

%Resi dual = 0
y2Lat = 0;
y2Lon = 0;

el se
% di sp( spr i nt f ( ' R2 = %f ' , R2) ) ;
%Comput e r esi dual bet ween measur ement and pr edi ct i on
y2Lat = l at 2Gps( k) - Lat 2Hat ( k) ;
y2Lon = l on2Gps( k) - Lon2Hat ( k) ;

%Updat e t he di r ect i on per i odi cal l y i f t he r equi r ement s ar e
%f ul f i l l ed
i f k > 250 && mod( k, 250) == 2 && . . .
i sempt y( f i nd( abs( Can2. hsYaw( k- 100: k- 1) ) > 2) ) &&
Can2. hsSpeedOver Gr ound( k- 100) > 15 && Can2. hsSpeedOver Gr ound( k- 50) > 15

di sp( spr i nt f ( ' V2 - > k = %d' , k) ) ;
di sp( spr i nt f ( ' Pr evi ous di r ect i on = %f ' , di r ect i on2) ) ;
di r ect i on2 = updat eDi r ect i on( Can2, Y2Opt , X2Opt , k) ;
di sp( spr i nt f ( ' Updat ed di r ect i on = %f \ n' , di r ect i on2) ) ;
end
end

%Updat e measur ement er r or covar i ance
S2 = P2 + R2;

%Cal cul at e Kal man Gai n
K2 = P2 / S2;

%Cor r ect t he est i mat ed posi t i on
Lat 2Opt ( k) = Lat 2Hat ( k) + K2 * y2Lat ;
Lon2Opt ( k) = Lon2Hat ( k) + K2 * y2Lon;

%Updat e t he er r or covar i ance
P2 = ( 1 - K2) * P2;

%I f t he di r ect i on i s not det er mi ned yet ?

59
el se

%Post er i or i est i mat i on i s equal t o t he measur ement
Lat 2Opt ( k) = l at 2Gps( k) ;
Lon2Opt ( k) = l on2Gps( k) ;

%Check i f t he di r ect i on can be det er mi ned now?
i f k > 4*50 && i sReadyToSet Di r ect i on( Can2, Gps2, k)
di r ect i on2 = set Di r ect i on( Gps2, k) ;
di r ect i onFl ag2 = SET;
end

end

%Get Mer cat or pr oj ect ed coor di nat es
[ X2Opt ( k) , Y2Opt ( k) ] = get Mer cat or ( Lat 2Opt ( k) , Lon2Opt ( k) ) ;

%Di f f er ence bet ween r adar det ect i on r ange and di st ance of t he act ual
GPS r ecei ver posi t i ons
GpsBet wRadar Lengt h = 5. 1;

navi gat i onDi st ( k) = cal cDi st ( Lat 1Opt ( k) , Lon1Opt ( k) , Lat 2Opt ( k) ,
Lon2Opt ( k) ) ;
Rel at i veDi st ( k) = navi gat i onDi st ( k) - GpsBet wRadar Lengt h;

i f Can2. hsSpeedOver Gr ound( k) == 0
est Radar Angl e( k) = 0;
el se
est Radar Angl e( k) = cal cul at eAngl eBet weenCar s( X1Opt ( k) , Y1Opt ( k) ,
X2Opt ( k) , Y2Opt ( k) , di r ect i on2) ;
end

end

[ Lat Di st Fr omRadar , LonDi st Fr omRadar ] =
cal cLat LongDi st ance( ( ( Can2. r adar 71B. t ar get Angl e( 13: end) ) ) , Can2. r adar 71B. t ar
get Range( 13: end) ) ;
[ Lat Di st Fr omKal man, LonDi st Fr omKal man] =
cal cLat LongDi st ance( est Radar Angl e( 1: end- 12) , Rel at i veDi st ( 1: end- 12) ) ;

[ RMSE_LAT, RMSE_LON, STD_LAT, STD_LON] =
cal cEr r or St at i st i cs( Lat Di st Fr omRadar , Lat Di st Fr omKal man, LonDi st Fr omRadar ,
LonDi st Fr omKal man) ;
di sp( spr i nt f ( ' r mse Lat er al = %f \ nr mse Longi t udi nal = %f \ n' , RMSE_LAT,
RMSE_LON) ) ;
di sp( spr i nt f ( ' st d Lat er al = %f \ nst d Longi t udi nal = %f \ n' , STD_LAT,
STD_LON) ) ;

ARRAY_START = 1;
ARRAY_SI ZE = l en- 12;
t i meAxi s = 20e- 3 * [ 0: ARRAY_SI ZE - ARRAY_START] ;

60

er asedLat Di st = Lat Di st Fr omKal man;
er asedLat Di st ( f i nd( Can2. r adar 71B. t ar get Range( 13: end) == 0) ) = NaN;
Lat Di st Fr omRadar ( f i nd( Can2. r adar 71B. t ar get Range( 13: end) == 0) ) = NaN;
f i gur e
pl ot ( t i meAxi s, Lat Di st Fr omRadar ( ARRAY_START: ARRAY_SI ZE) ) ;
hol d on
pl ot ( t i meAxi s, er asedLat Di st ( ARRAY_START: ARRAY_SI ZE) , ' r ' ) ;
l egend( ' Radar - Camer a' , ' Kal man' ) ;
xl abel ( ' t i me ( s) ' ) ;
yl abel ( ' Lat er al di st ance ( m) ' ) ;
t i t l e( ' Lat er al Di st ance' ) ;

er asedLonDi st = LonDi st Fr omKal man;
er asedLonDi st ( f i nd( Can2. r adar 71B. t ar get Range( 13: end) == 0) ) = NaN;
LonDi st Fr omRadar ( f i nd( Can2. r adar 71B. t ar get Range( 13: end) == 0) ) = NaN;
f i gur e
pl ot ( t i meAxi s, LonDi st Fr omRadar ( ARRAY_START: ARRAY_SI ZE) ) ;
hol d on
pl ot ( t i meAxi s, er asedLonDi st ( ARRAY_START: ARRAY_SI ZE) , ' r ' ) ;
xl abel ( ' t i me ( s) ' ) ;
yl abel ( ' Longi t udi nal di st ance ( m) ' ) ;
t i t l e( ' Longi t udi nal Di st ance' ) ;

r adar Tar get Angl e = Can2. r adar 71B. t ar get Angl e( 13: end) ;
er asedAngl e = est Radar Angl e;
er asedAngl e( f i nd( Can2. r adar 71B. t ar get Range( 13: end) == 0) ) = NaN;
r adar Tar get Angl e( f i nd( Can2. r adar 71B. t ar get Range( 13: end) == 0) ) = NaN;
f i gur e
pl ot ( t i meAxi s, r adar Tar get Angl e( ARRAY_START: ARRAY_SI ZE) ) ;
hol d on
pl ot ( t i meAxi s, er asedAngl e( ARRAY_START: ARRAY_SI ZE) , ' r ' ) ;
xl abel ( ' t i me ( s) ' ) ;
yl abel ( ' angl e ( degr ees) ' ) ;
t i t l e( ' Tar get Angl e' ) ;

[ Lat Hi st , Lat Er r or ] = hi st ( Lat Di st Fr omRadar ( ARRAY_START: ARRAY_SI ZE) -
er asedLat Di st ( ARRAY_START: ARRAY_SI ZE) , 100) ;
[ LonHi st , LonEr r or ] = hi st ( LonDi st Fr omRadar ( ARRAY_START: ARRAY_SI ZE) -
er asedLonDi st ( ARRAY_START: ARRAY_SI ZE) , 100) ;
[ AngHi st , AngEr r or ] = hi st ( r adar Tar get Angl e( ARRAY_START: ARRAY_SI ZE) -
er asedAngl e( ARRAY_START: ARRAY_SI ZE) , 100) ;
Lat Hi st = ( Lat Hi st / sum( Lat Hi st ) ) * 100;
LonHi st = ( LonHi st / sum( LonHi st ) ) * 100;
AngHi st = ( AngHi st / sum( AngHi st ) ) * 100;

f i gur e;
bar ( Lat Er r or , Lat Hi st , 1. 0) ;
t i t l e( ' Lat er al Er r or Hi st ogr am' ) ;
xl abel ( ' met er s' ) ;
yl abel ( ' per cent age ( %) ' ) ;

61

f i gur e;
bar ( LonEr r or , LonHi st , 1. 0) ;
t i t l e( ' Longi t udi nal Er r or Hi st ogr am' ) ;
xl abel ( ' met er s' ) ;

f i gur e;
bar ( AngEr r or , AngHi st , 1. 0) ;
t i t l e( ' Tar get Angl e Er r or Hi st ogr am' ) ;
xl abel ( ' degr ees' ) ;

Relative Distance Measurement Using GPS and Internal Vehicle Sensors

Enviado por

Dados do documento

Descrição original:

Título original

Direitos autorais

Formatos disponíveis

Compartilhar este documento

Compartilhar ou incorporar documento

Opções de compartilhamento

Você considera este documento útil?

Este conteúdo é inapropriado?

Direitos autorais:

Formatos disponíveis

Relative Distance Measurement Using GPS and Internal Vehicle Sensors

Enviado por

Direitos autorais:

Formatos disponíveis

RelativeDistanceMeasurement

Kommentar [mp1]: Ifthereportis

SOF Identifier RTR Control Data CRC ACK EOF

Vector CANcaseXL is a USB interface, which is capable of processing CAN messages. It is

Field Example Comments

<14> TimesinceDGPS inseconds

Field Example Comments

Field Example Comments

_ = (N +b) cos cos z z = orcton

= (N + b) cos sin z = orcton

o = scmi mo]or cortb oxis (cquotoriol roJius)

Initialization of the states

Ineitial navigation foimulas foi piioii position estimation

[ t empToken, t empSt r i ng] = st r t ok( t hi sLi ne) ;

el apsedSeconds = el apsedTi me - ( el apsedMi nut es * 60) ;

%Copy t he l at i t ude and l ongi t ude i nf or mat i on t o ar r ays

%St ar t sensor f usi on

di sp( spr i nt f ( ' Updat ed di r ect i on = %f \ n' , di r ect i on1) ) ;

[ Lat 2Hat ( k) , Lon2Hat ( k) ] = updat eLat Lon( Lat 2Opt ( k- 1) , Lon2Opt ( k- 1) ,

Você também pode gostar