136 L. P ˇ reu ˇ cil and R. M ´ azl significantly within the last decade, it can still fail from its’ principle and due to inaccessibility of the RF signal from satellites in particular situations. These situations are consideredfor critical and should be avoided from safety reasons. The GPS signal is not available in manylocations due to signal shielding e.g. in urban areas, underground spaces, inside buildings, tunnels, deep and narrowvalleys, etc. While the GPS is not able to meet the basic requirements in terms of integrity and availability in ageneral sense, therefore the localization control system can’trely exclusively onthe GPS. The vehicle canbe equipped not only withasatellite receiver of navigation data, butalso it can employother on-board sensors e.g. odometry or inertial navigation sensor.Each of these sensors have their ownspecific features and disadvantages and compared to the GPS theymainly employthe dead-reckoning principle to obtain some suitable navigation information. Application of sensors forinertial navigation (gyros,accelerometers, tilt sensors) does not depend on operational condition. Their usage has to takeinto account many fluctuating parameters (e.g. sensor drift, bias, non-linearity). Precise estimation of the sensor parameters has direct impacts on desirable accuracyand reliability of the localization system in this case. Dead reckoning by the means of odometry fails if insufficient adhesion between the vehicle wheel and ground occurs, due to adhesion or type of the surface (e.g. rain, ice or leaves, soft-type of surface). Quality of adhesion impairs particularly when the vehicle accelerates or brakes. Authors in [1] evaluate the vehicle speed based on afuzzy inference system and neural networks using differences and rapid changes between odometry sensors joined with multiple wheels. There are manyreferences in robotics field on data fusion from gyroscopes, ac- celerometers and odometry.The most of them solvedata fusion problems employing the Kalman filter [2], or PDAF techniques [7]. Our approach doesn’tuse classical state vector to determine current measured values and their errors. Instead of this we recognize occurrence of error situation directly and repair these situations by interpolation methods subsequently. Aiming to achieve better final performance of the navigation system, we intro- duce an application-oriented approach to fusion of data measured by the odometry and onboard accelerometer in the following. The presented approach is drivenby abelief, that the most typical errors of these sensors are uncorrelated. Significant odometry errors occur during acceleration or braking intervals, which can be suc- cessfully discovered by accelerometers. On the other hand, long-lasting and more or less constant motion speeds are very good preconditions for error-free odometry measurement. Then, accelerometers are typically useless in these cases. Therefore, combination of both the sensor types has been assumed to improve the quality of the localization solution. 2Problem Setup Navigation of the vehicle consists of determination of forward position on its’ path and in precise detection of motion direction, in particular in curves. If the GPS signal Vehicle Localization Using Inertial Sensors and GPS 137 is permanently available, accuracyofthe GPS is sufficient even for determination of changes between twotracks after passing acurveofcrossing. Wheneverthe signal of the GPS is lost, the correct tracking of the vehicle position has to be maintained. One of the worst-case situations comes about when the vehicle drivesthrough ashielded region doing some maneuvers. The GPS needs relatively long time for retrievalof actual position again. This problem situation can be partially overcome by application of inertial sen- sors, principally gyros and accelerometers with active axis oriented perpendicularly to the motion direction. The gyros can be used for preserving information about heading. Forprecise solution of heading in along-term period correction mecha- nisms have to be employed. One of the powerful approaches is amap-matching algorithm [3], which desires at least an estimate of the traveled distance to deter- mine the position and heading. The odometry can be used for this purpose, butit suffers from randomly occurred, unpredictable and almost unbound errors due to insufficient wheel adhesion. On the other hand, the main constrains of the inertial system navigation system performance to estimate the traveled distance are set by the finite (and limited) resolution of the sensors themselves. Even asmall butpermanent offset error in acceleration will be integrated and results in aremarkable error in speed. After double integration it raises inalarge error in distance.Therefore,very precise and low offset sensors and error correction mechanisms (feedback algorithms) are necessary to obtain an acceptable inertial navigation platform. Therefore our contribution deals Accelerometer Odometry GPS Slippage detection Slippagecorrection Switch strategy by availability of GPS (GPS /ODO) Travelleddistance Offset Correction Calibration Fig.1. Principal overviewofthe proposed method. with adesign of arobust feedback algorithm to perform the double integration of acceleration from accelerometer with acceptable final errors enabling to use the method output as atemporal-substitute dead-reckoning system. Forsimplification, in the first steps, the method has been designed for the case of accelerometer in question with its’ active axis having mounted collinear with the vehicle driving direction. Global overviewofthe proposed method and mutual interconnections of particular sensors in the approach to distance measurement are illustrated in the Fig. 1. 138 L. P ˇ reu ˇ cil and R. M ´ azl 2.1 Data Analysis and Preprocessing The approach to estimation of the traveled distance is based on processing of signal from odometer and accelerometers. As the odometer measurements may be pro- cessed directly,the acceleration data are corrupted by remarkable noise (caused by vehicle vibrations while driving and/or noise of the sensing system itself). 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 -2 -1 0 1 2 x10 4 Frequence(Hz) Fig.2. Frequencyspectrum of the accelerometer signal. Even thought data from accelerometer do not need to be filtered before further processing (integration itself provides astrong low-pass filtration effect), we have been interested in filtration of the signal for experiment evaluation purposes. The intention wastofind suitable structure and parameters of afilter providing relatively smooth shape of the signal without damaging the integral (mean) value of the origin.The parameter has to be determined asatrade-offbetween the smoothness (but also the levelofdegradation) of the signal and the levelofnoise in the output signal. To optimize the achievedresults, twofiltering techniques have been applied; the former uses sliding- windowaveraging as the latter employs standard 4th-order Butterworth low-pass filter. The Fig. 3introduces not only the influence of particular parameters in the process of filtration, butitalso illustrates some other issues related to the used sensor offset. As the vehicle speed can be obtained by simple integration of the acceleration along time: v ( t )= t 0 ( a ( t ) − offset( t )) dt (1) where a stands for measured acceleration and offset for actual offset of acceler- ation sensor,being basically an unknown butconstrained function of time. The Fig. 3compares direct (reference) speed measurements obtained from the GPS and integration of the acceleration after filtration. The precision of the pre- ceeding process result strongly depends on the exact estimation of the sensor offset. Unfortunately,the sensor offset is highly variable with time and it is not possible to estimate its’ exact model. Moreover, the sensor offset depends on the past behaviour of the vehicle, where its’ evolution is drivenbyunknown transfer characteristic with substantial hysteresis as can be seen in the Fig. 4. The integration of data plotted in the Fig. 3assumes that an accelerated body performs abounded motion with final return to the original position (a forth-and- back motion). This additional information allows us to determine an average offset Vehicle Localization Using Inertial Sensors and GPS 139 along themeasured data sequence. However, some deviations ofintegrated datafrom the reference GPS shape are clearly visible (e.g. the amplitude of the integrated data is lower than the reference). 0 50 100 150 200 250 300 350 400 450 500 -15 -10 -5 0 5 10 15 Time [s] Speed[m/s] GPS Signal from accelerometers afterintegration → raw /Butterworthfilter, seedetails 0.1Hz 0.3Hz More then 1Hz 140 145 150 155 160 165 170 8 8.2 8.4 8.6 8.8 9 GPS Detail view -filtration Fig.3. An example of signal degradation after afiltering. -10 -8 -6 -4 -2 0 2 4 6 8 10 -10 -8 -6 -4 -2 0 2 4 6 8 10 SpeedfromGPS [m/s] Speedfromaccelerometer after1.integration [m/s] Fig.4. Comparison of the vehicle speed from GPS vs. speed via acceleration The shown behavior givesgood reasons for the exact determination of asensor offset to be the central topic in the following sections. 3Data Fusion To obtain the travelled distance value, data fusion based on GPS, odometry and accelerometer has to be introduced. The fusion method has to respect the possibility of temporal dropouts in performance of each sensor.Suppose, the GPS signal is mostly available and therefore our effort is mainly targeted to bridge the GPS-dark areas. The recent research has shown that one of the good ways for fusion of the odometry and accelerometer sensor data is arule-based mechanism. The desired 140 L. P ˇ reu ˇ cil and R. M ´ azl behavior is to prefer odometry data to accelerometers as soon as GPS measurement is not available. As the GPS is lost the navigation system has to ensure immediate and smooth switching to inertial sensors and odometry.The basic strategy for fusion and data processing is sketched in the previous Fig. 1. The odometry data are generally reliable as we suppose precise vehicle wheel calibration and no slippage between the wheel and the ground. Calibration of the odometry seems to be astraightforward task to be performed wheneverthe GPS is in operation and even together with an existing map of the environment. As long as the odometer has been properly calibrated (or continuously recali- brated during movements), it is possible to switch the navigation system from GPS to odometer.Unfortunately,the odometer itself can still cause large and cumulative errors due to wheel slippage. Our method for data fusion solves in particular some issues associated with wheel slippage. The core idea of the approach is based on different essence of errors, which both the odometry and the accelerometer give.The odometer typically fails in relatively short time intervals mainly during acceleration or braking periods. These situations can be successfully handled by accelerometer as long as current sensor parameters for integration are known at atime (in par- ticular its’ last value of the offset). The odometer and accelerometer can serveas supplementary sensing pair substituting each other,ifdesired. During ashort time period, wheneverneither GPS, nor reliable odometry data are accessible, the measurement of the traveled distance relies only on integration of acceleration. Precondition to achieve reliable results of such integration stands in estimation of the current offset, as mentioned above.Besides that, the estimated offset can also be used for recognition of the odometry slippage. The final algorithm works in separated time frames (the time length of one basic frame is typically in order of seconds), while in scope of which offsets of the accelerometer are estimated. This is done by the means of LSQ method, which evaluates offset of accelerometer in order to reach aminimal difference between speeds obtained from odometer and integrated acceleration with subtracted offset. offs acc =arg min offs acc t 2 t k = t 1 v odo ( t k ) − t k t 1 ( a acc ( t ) − offs acc ) dt + v t 1 2 (2) where t 1 and t 2 stands for time-boundary of processed time frame, v odo denotes the odometric speed, a acc meansthe current sensor value from accelerometer, offs acc stands for estimated current sensor offset for the time frame and v t 1 is the initial velocity (at the beginning of atime frame) computed from acceleration. The minimization process returns estimation of the apparent accelerometer off- set. Provided that estimated offset has asharp slope, itisanindication of the odometry slippage (e.g the estimated accelerometer offset is significantly greater or lesser than astandard value which varies slowly). This situation is illustrated in Fig. 5 In case, that progress of the apparent accelerometer offset is smooth, the offset is treated as areal accelerometer offset overthe whole time frame and the final traveled distance is computed directly by double integration of accelerometer data Vehicle Localization Using Inertial Sensors and GPS 141 0 10 20 30 40 50 60 70 80 90 100 0.2 0.3 0.4 0.5 Time(s) Averageacceleration correction(m/s2) Intervalswithout slippage Rejected intervals -interpolated Fig.5. Selection of rejected intervals and interpolation. with subtracted estimated offset. This can also be seen as direct locking of evaluation of accelerometer-based distance onto odometry (via aminimization process). If anextreme offset (a slippage has occured)is detected, the whole corresponding interval is marked as odometry-unreliable one and pure accelerometer is used for the calculation of traveled distance (with no estimation of the offset from odometry). The evaluation of accelerometer offset has to be treated in another wayinorder to guarantee proper conditions for the following double integration process in this case. To solutionof this probleman extrapolation (orinterpolation for slightlytime-shifted processing) of preceding offset values before slippage (odometry-reliable interval) can be applied. The 3rd order splines provide reasonable results for interpolation, see Fig. 5. In fact, the traveled distance estimation always uses accelerometer data and the major differences are only in the wayofdetermination of current accelerometer offset. Then, the final traveled distance is easily computed by double integration with respect to the average offset of the accelerometer. 4Experiments The fusion algorithms for odometry and acceleration data were designed for use with train vehicles to serveascomplementary substitute to GPS-based vehicle position- ing. Experimental data were gathered with asetup carrying an incremental optical odometer offering aresolution of 400 pulses/rev. and industrial accelerometer type CrossbowCXL01LF.The exact reference forward position of the vehicle has been obtained from differential GPS receivers operating in RTK(Real Time Kinematics) mode with the order of about –0.01m accuracy. As the RTKmode of the GPS is not generally suitable for wide practical application due to its’ slowness and additional accessories needed, it is very useful for evaluation of the thereunder achievedresults. The real experiment wasperformed on areal railwaytrack with intentionally created slippage fields (by application of high accelerations and brakings) in specific parts of the path. This situation can be noted in the following Fig. 6approximately by the 60th sec of the experiment runtime (circled). The measured odometry and acceleration data were provided to the described fusion algorithm, the quality evaluation of which has been done by comparison with the GPS measurements and which were gathered synchronously.Therefore, the 142 L. P ˇ reu ˇ cil and R. M ´ azl 0 100 200 300 400 500 600 -15 -10 -5 0 5 10 15 20 Time [s] Accelerometer(1.int) Odometry(1.der) Direct GPS(lin.interpolation) Speed[m2/s] Fig.6. Input data before processing. comparison of the real travelled distance provided by the GPS with the result of our approach has been straightforward. The most important step of the described approach stands in the precise estima- tion of accelerometer offset; mainly in time frames wheneverodometry fails. The final result of integration with respect to the estimated offset can be seen in the Fig. 7. The long-term accuracydepends on proper calibration of odometry.This means that the results of data fusion can’tprovide better accuracythan the odometry. However, the major odometry error has been successfully corrected. The remaining distance error after application of suited two-stage integration of acceleration with respect to current sensor offset is less than 1m after 9minutes drive (see Fig. 7). 5 10 15 20 30 40 50 60 70 80 90 100 0 Time (s) Speed(m/s) Odometry Speed odometru Speed aftercorrection Real speedfromGPS 0 100 200 300 400 500 600 -1 -0.5 0 0.5 1 Distanceerrors [m] Time [s] Fig.7. Result of the correction algorithm and remaining distance error. Vehicle Localization Using Inertial Sensors and GPS 143 We suppose, remaining distance error arises from inaccuracyofodometer if the vehicle drivesthrough curves. Turns induce achange of wheel effective diameter due the centripetal and centrifugal forces. In order to test robustness of designed approach the designed approach has also been tested with partially artificial test data sets. These data sets are based on original real data, butadditional wheel slippages are added by simulation in odometry data. The Fig. 8illustrates three simulated slippages besides of first real slippage from acceleration. The first simulated slippage is quite similar to the real one, the second one is very short butheavy and the last slippage simulates ahard-breaking state of the vehicle. The following Fig. 9shows result errors after performingthefusion of accelerom- eter and odometry data with elimination of slippages. The presented approach proofs to be very efficient for higher odometry errors in time frame from 1to15seconds. 0 50 100 150 200 250 300 0 2 4 6 8 10 12 14 16 18 20 Time(s) Speed(m/s) Odometry Speed Speed aftercorrection Real speed from GPS Fig.8. Results with additional artificial slippage in odometry. The only bottleneck of the introduced algorithm determined in the experiments can be seen in imperfect detection of extremely small and narrowodometry errors. The primary cause for this is likely to be the impossibility to identify small changes in accelerometer offset to determine these micro-slippages. 5Conclusion The presented contribution shows one of the possible and robust ways for utilization of inertial sensors as short and medium time substitute of the satellite navigation system. Long-term precision depends on calibration of the odometer,nevertheless local odometer error induced by wheelslippageispossibletobesuccessfullydetected and treated using an accelerometer.The described method is under development towards extension for full 2D localization and it is expected to be targeted on 144 L. P ˇ reu ˇ cil and R. M ´ azl 0 50 100 150 200 250 300 -1 0 1 2 Time (s) Distance errors(m) 0 50 100 150 200 250 300 -0.2 -0.1 0 Time(s) Speed (m/s) 0 50 100 150 200 250 300 -1 0 1 2 Time (s) Distance errors(m) 0 50 100 150 200 250 300 -0.2 -0.1 0 Time(s) Speed (m/s) Fig.9. Speed and distance differences between corrected and GPS data. improvement of the estimation mechanism for acceleration sensor offset with the objective to achieve higher precision in path-integration. It is assumed, that possible solution to this might lead via dynamic optimisation of processing frames size and combination of their different sizes and/or combining with the sliding-window approach. Acknowledgement The presented research has been supported within the IST-2001-FET framework under project no. 38873 "PeLoTe". The work is also supported by the Ministry of Education of the Czech Republic within the frame of the projects "Decision making and Control for Manufacturing" number MSM 212300013. References 1. B.Allotta, P.Toni, M.Malvezzi, P. Presciani, G.Cocci, V.Colla, “Distance”, Proc. of World Congress on Railway Research 2001,Koeln, Germany, 2001 2. B.Barsahn, Hugh F. Durrant-Whyte, “Inertial Navigation System for mobile robots”, IEEE Transaction on robotics and automation,vol.11 no. 3, pages 328–342, June 1995 3. A. Filip, H.Mocek, L.Bazant, “GPS/GNSS Based Train Positioning for safety Critical Applications”, Signal +Draht [93],vol.5, pp.16–21 (in German) pp.51-55 (in English) 4. A. Filip, L. Bazant, H. Mocek ,J.Taufer,and V. Maixner,“Dynamic properties of GNSS/ INS based trainposition locator for signalling applications”, The proceedings of the Comprail 2002 conference,Greece, 2002 5. A.Lawrence, Modern Inertial Technology -Navigation Guidance,and Control,ISBN 0-387-98507-7, Springer 1998. 6. R. M ´ azl, “Preliminary study for the train locator project -accelerometer and odometer data fusion”, Research report no. GLR 66/02, CTU,FEE, Dep. of Cybernetics,The Gerstner Lab for Intelligent Decision Making and Control, Prague, 2002. (Czech lang.) 7. Y.Bar-Shalom, Thomas E. Fortmann, “Tracking and Data Association”, Volume 179 in Mathematics in science and engineering,ISBN 0-12-079760-7, Academic press, 1988 An Experimental Study of Localization Using Wireless Ethernet Abstract. 1Introduction [...]... -3 0 -4 0 -5 0 -6 0 -7 0 -8 0 -9 0 -1 00 -3 0 -4 0 -5 0 -6 0 -7 0 -8 0 -9 0 -1 00 -2 0 -1 5 -1 0 -5 0 5 10 15 -8 -6 -4 -2 0 2 6 4 8 10 -2 0 -1 5 -1 0 -5 0 5 (a) 10 15 20 -8 -6 -4 -2 2 0 4 6 8 10 (b) Fig 3 (a) Signal strength recorded by robot Fly over two complete circuits of the environment (b) Signal strength recorded by robot Bug over a similar circuit Level (dB) Level (dB) -3 0 -4 0 -5 0 -6 0 -7 0 -8 0 -9 0 -1 00 -3 0 -4 0 -5 0... Level (dB) -3 0 -4 0 -5 0 -6 0 -7 0 -8 0 -9 0 -1 00 -3 0 -4 0 -5 0 -6 0 -7 0 -8 0 -9 0 -1 00 10 10 5 -2 0 -1 5 -1 0 0 -5 0 5 10 -5 15 (a) 20 -1 0 5 -2 0 -1 5 -1 0 0 -5 0 5 10 -5 15 20 -1 0 (b) Fig 4 Interpolated signal strength maps generated using the filters K1 and K1 ·K2 (described in the text) The maps were generated using the sample set shown in Figure 2(a) doors, and so on Some of the variation in the signal strength... the corridors and offices, opening and closing 148 A Howard, S Siddiqi, and G.S Sukhatme -4 0 -5 0 -5 0 -5 0 -6 0 -7 0 Level (dB) -3 0 -4 0 Level (dB) -3 0 -4 0 Level (dB) -3 0 -6 0 -7 0 -8 0 -7 0 -8 0 -9 0 -6 0 -9 0 -1 00 0 6 12 18 24 30 36 42 -1 00 48 -8 0 -9 0 0 5 10 Time (hours) 15 20 25 -1 00 -1 80 30 -1 20 Range (m) (a) -6 0 0 60 120 180 Orientation (degrees) (b) (c) Fig 2 Signal strength measurements for wireless beacon... Wireless Ethernet 10 151 10 True Estimated 8 Location error (m) 6 4 2 0 -2 1 0.1 -4 -6 -8 -2 0 0.01 -1 5 -1 0 -5 0 5 10 10 15 0 10 20 30 40 50 60 Distance travelled (m) 70 80 90 0 10 20 30 40 50 60 Distance travelled (m) 70 80 90 0 10 20 30 40 50 60 Distance travelled (m) 70 80 90 20 10 True Estimated 8 Location error (m) 6 4 2 0 -2 1 0.1 -4 -6 -8 -2 0 0.01 -1 5 -1 0 -5 0 5 10 10 15 10 True Estimated 8 Location... error (m) 6 4 2 0 -2 1 0.1 -4 -6 -8 -2 0 0.01 -1 5 -1 0 -5 0 5 10 15 20 Fig 5 Localization results using different combinations of Wi-Fi and contact sensing The plots on the left show the estimated robot trajectory (the true trajectory is indicated by the ‘+’ symbols); the plots on the right show the error in the pose estimate as a function of the distance travelled by the robot (Top) Wi-Fi sensing only... DABT6 3-9 9-1 -0 0 15 and 5- 3 950 9A (via UPenn) under the Mobile Autonomous Robot Software (MARS) program References 1 S Arulampalam, S Maskell, N Gordon, and T Clapp A tutorial on particle filters for on-line non-linear/non-gaussian bayesian tracking IEEE Transactions on Signal Processing, 50 (2):174–188, Feb 2002 2 P Bahl and V N Padmanabhan RADAR: An in-building RF-based user location and tracking system... Wi-Fi-based localization has been incorporated into the Player robot device server [5] , which can be downloaded from the Player/Stage web-site [4] The data-sets used in this paper are also available on the Radish (Robotics Data Set Repository) web-site [7] An Experimental Study of Localization Using Wireless Ethernet 153 Acknowledgments This work is sponsored in part by DARPA grants DABT6 3-9 9-1 -0 0 15. .. steady-state error, however, is only 0.26 ± 03 m; better than that obtained using the Wi-Fi sensor alone The third and final row in Figure 5 shows the results of combining Wi-Fi and contact sensing Here, convergence is rapid, and 152 A Howard, S Siddiqi, and G.S Sukhatme the steady-state error is only 0. 25 ± 0.02 m It would appear that these two sensors complement each other extremely well: Wi-Fi ensures... the aisles stays more or less the same S Yuta et al (Eds.): Field and Service Robotics, STAR 24, pp 155 –164, 2006 © Springer-Verlag Berlin Heidelberg 2006 156 M Fiegert and C.-M De Graeve Fig 1 Local occupancy grid from laser range measurements in a canteen No line features are present and the dots are not invariant, because chairs are moved, and because of sensor noise Nevertheless a structure is present,... Wi-Fi Signal Strength Figure 2(a) shows a plot of the signal strength recorded by one of the robots over a 48 hour period The robot was located adjacent to beacon A in Figure 1, and recorded the signal strength for beacon B The period captured includes two full working days, with people moving about in the corridors and offices, opening and closing 148 A Howard, S Siddiqi, and G.S Sukhatme -4 0 -5 0 -5 0 . (dB) -2 0 -1 5 -1 0 -5 0 5 10 15 20 -1 0 -5 0 5 10 -1 00 -9 0 -8 0 -7 0 -6 0 -5 0 -4 0 -3 0 Level (dB) -2 0 -1 5 -1 0 -5 0 5 10 15 20 -1 0 -5 0 5 10 -1 00 -9 0 -8 0 -7 0 -6 0 -5 0 -4 0 -3 0 Level (dB) -2 0 -1 5 -1 0 -5 0 5 10 15 20 -1 0 -5 0 5 10 -1 00 -9 0 -8 0 -7 0 -6 0 -5 0 -4 0 -3 0 Level. circuits of the environment. (b) Signal strength recorded by robot Bug overasimilar circuit. -2 0 -1 5 -1 0 -5 0 5 10 15 20 -1 0 -5 0 5 10 -1 00 -9 0 -8 0 -7 0 -6 0 -5 0 -4 0 -3 0 Level (dB) -2 0 -1 5 -1 0 -5 0 5 10 15 20 -1 0 -5 0 5 10 -1 00 -9 0 -8 0 -7 0 -6 0 -5 0 -4 0 -3 0 Level. Strength 148 A. Howard, S. Siddiqi, and G .S. Sukhatme -1 00 -9 0 -8 0 -7 0 -6 0 -5 0 -4 0 -3 0 0 6 12 18 24 30 36 42 48 Level (dB) Time (hours) (a) -1 00 -9 0 -8 0 -7 0 -6 0 -5 0 -4 0 -3 0 0 5 10 15 20 25 30 Level (dB) Range