Improved GPS Sensor Model for Mobile Robots in UrbanTerrain
Daniel Maier* and Alexander Kleiner*
Abstract— Autonomous robot navigation in out-door scenarios gains increasing importance in vari-ous growing application areas. Whereas in non-urbandomains such as deserts the problem of successfulGPS-based navigation appears to be almost solved,navigation in urban domains particularly in the closevicinity of buildings is still a challenging problem. Insuch situations GPS accuracy significantly drops downdue to multiple signal reflections with larger objectscausing the so-called multipath error. In this paper wecontribute a novel approach for incorporating multi-path errors into the conventional GPS sensor modelby analyzing environmental structures from onlinegenerated point clouds. The approach has been val-idated by experimental results conducted with an all-terrain robot operating in scenarios requiring close-to-building navigation. Presented results show thatpositioning accuracy can significantly be improvedwithin urban domains.
I. Introduction
Autonomous robot navigation in outdoor scenariosgains increasing importance in various growing applica-tion domains, such as reconnaissance, urban search andrescue, bomb disposal, transportation of goods, assis-tance for disabled people, driving assistance systems, andmany more. Successful systems for GPS-based outdoornavigation have been demonstrated in the past, as for ex-ample during the DARPA Grand Challenge, and DARPAUrban Challenge. In these domains perturbed GPS read-ings are comparably seldom due to sufficient clearancearound the vehicle. In contrast, there are other domainsrequiring mobile robots to navigate arbitrarily close tobuildings, as for example when the task involves enteringan unknown building through a geo-referenced entrance.Depending on the complexity of the environment, andthe robot’s ability to recognize targets, the quality ofthe estimated pose has to be sufficiently high. However,particularly in close vicinity of buildings, GPS accuracysignificantly drops down due to multiple signal reflectionon larger objects causing the so-called multipath error.
A commonly deployed technique to compensate forthese errors is to fuse multiple sensor readings weightedby their confidence with a filter. In case of GPS, confi-dence is gained from the HDOP (Horizontal Dilution ofPrecision) value, a measure computed according to thegeometric constellation of GPS satellites. Since HDOPdoes not model multipath propagation, overconfident
*Department of Computer Science, University of Freiburg,Georges-Kohler-Allee 52, D-79110 Freiburg, Germany,{maierd,kleiner}@informatik.uni-freiburg.de
measurements are fused by the filter leading to highly in-accurate position estimates particularly in urban terrain.Figure 1 (a) illustrates the multipath effect between twoclose buildings, and Figure 1 (b) plots the correspondingGPS error (blue line) together with the HDOP-based 1σbound (red line) over time. As can be seen, conventionalerror bounds do not hold in this particular situation.
0 400 800 1200 1600
10
20
30
40
Time in s
Dis
tanc
ein
m
σGPS
distGPS
shown in (a)
(a) (b)
Fig. 1. (a) The effect of multipath when traveling between twobuildings. The GPS measurements (orange dots) differ significantlyfrom the true trajectory of the robot (yellow). (b) The correspond-ing GPS error (blue line) together with the HDOP-based 1σ bound(red line) over time.
Several methods have been proposed to estimate theuncertainty of GPS measurements past. Abbot and Pow-ell [1] investigated the contribution of GPS, IMU, andodometry on the navigation performance of various landvehicles. They found that the overall position error isdominated by the quality of GPS. One of the simplestways to model GPS uncertainty is to assume a constantvalue, for instance used by Goel et al. [2]. Anothermethod of modeling the GPS uncertainty is to dynami-cally adjust the covariance according to a parameter. Forexample, Thrapp et al. [3] used the number of availablesatellites. Other approaches were proposed to detectmultipath errors. Caron et al. [4] introduced contextualvariables to define the validity of IMU and GPS. Giremuset al. [5] used a rao-blackwellized particle filter to detectabrupt changes in the state model. However, their ap-proach was only evaluated with simulated measurements.Brenneman et al. [6] used a statistical test on the signalof an array of GPS antennas to detect the presenceof multipath. Common to approaches relying solely onthe GPS receiver’s output is that they cannot detectmultipath presence if only the multipath replica but notthe direct signal itself is propagated to the GPS receiver.
In this paper we contribute a novel approach for incor-porating multipath effects into the conventional GPS sen-sor model, and by this, increasing positioning accuracywithin urban domains. This is carried out by generating
2010 IEEE International Conference on Robotics and AutomationAnchorage Convention DistrictMay 3-8, 2010, Anchorage, Alaska, USA
978-1-4244-5040-4/10/$26.00 ©2010 IEEE 4385
online a 3D representation of the robot’s surroundings,and detecting obstructed satellites by performing ray-traces through this representation. The improved sensormodel is also computed based on the HDOP, however,from non-obstructed satellites only. The approach hasbeen applied with an Unscented Kalman filter (UKF)fusing data from odometry, IMU, and GPS, using thenew uncertainty measure for improving pose estimation.
Experimental results presented in this paper wereconducted with a telemax robot-based system that hasbeen designed for the TechX challenge [7] hold 2008 inSingapore. Among other tasks, such as elevator handling,stair negotiation, and target detection, one critical taskwas about approaching the target building entrance viaa stair, which required accurate pose estimation close tohigh building structures.
The remainder of this paper is structured as follows.Section II describes the integration of GPS and odometryinformation, and Section III introduces the improvedsensor model. In Section IV experimental results arepresented, and concluded in Section V.
II. Integration of GPS and Odometry
In this section the unscented Kalman filter for the inte-gration of GPS and odometry data, and the conventionalsensor model for GPS integration are described.
A. The Unscented Kalman Filter (UKF)The UKF [8] allows the estimation of the state of a
dynamic system given a sequence of observations andcontrol inputs. Observations originating from differentsensors are typically fused to obtain a more robustestimate of the state. Let xt be the state, ut the controlinput, and zt the observation at time t. Furthermore,assume that state transitions are given by a functiong and observations by a function h, both corrupted byGaussian noise. That is,
xt = g(xt−1, ut−1) + �t , (1)zt = h(xt) + δt , (2)
where �t and δt are zero-mean Gaussian noise variableswith covariance Qt and Rt, respectively.
Because h and g are potentially non-linear, a Gaussianvariable passed through either of them loses its Gaussiancharacter. Hence, the UKF implements a stochastic lin-earization of g and h, called unscented transformation,around the pose estimate computed in the previoustime step t − 1. Please refer to [8] for a more detaileddescription.
B. Conventional GPS Sensor ModelThere are two major factors affecting the accuracy
of GPS. a) The geometric constellation of the satel-lites represented by a numeric value termed Dilutionof Precision (DOP). b) The errors in the pseudorangeestimation, referred to as user-equivalent range error(UERE), which is typically expressed by its standard
deviation σUERE . Therefore, the error of the positionestimate is approximated by
Positioning Error(1σ) = DOP · σUERE . (3)
Standard deviation σUERE is affected by multiplesources. First, atmospheric effects influence the prop-agation speed of the GPS signal. Second, transmittedpositions of satellites (the ephemeris) can be inaccurate.Third, the satellites’ internal clock may experience driftand noise and is subject to relativistic effects. Fourth,there are multipath effects in case reflected replica ofthe GPS signal are transmitted to the receiver. Whereasthe first three error sources can mostly be compensatedby using DGPS (Differential GPS), the fourth sourceparticularly dominates the error in urban terrain.
DOP values can be calculated for various levels accord-ing to the specific requirements of the task. For example,the Horizontal DOP (HDOP) captures the influence ofthe satellite constellation on the position estimate in thehorizontal plane, i.e., ignoring the vertical component,and is commonly used on unmanned ground vehicles(UGVs). Algorithm 1 describes the HDOP calculationaccording to [9]. From Equation 3 we obtain a model
DOP calculation for a set of n satellites:
1 H =
0
B@a1x a
1y a
1z 1
......
......
anx a
ny a
nz 1
1
CA , where
(aix, a
iy, a
iz) = sphere2cartesian(θi, φi, 1)
is a unit vector in direction of the i-th satellite
2 M = (HTH)−1, where M is symmetric with
M =
0
BBBBB@
σ2x σ
2xy σ
2xz σ
2xt
... σ2y σ
2yz σ
2yt
...... σ
2z σ
2zt
......
... σ2t
1
CCCCCA
3 HDOP =q
σ2x + σ2
y
Algorithm 1: Dilution of Precision
for determining the covariance of the GPS position inthe horizontal plane. Therefore, DOP is replaced withthe HDOP value and the whole expression is squaredto obtain the covariance from the standard deviation.A slightly more sophisticated model reflecting both erroramplitudes in East and North direction can be developedby:
R =�
σ2x σ2
xy
σ2xy σ2
y
�· σ2
UERE , (4)
where the σ-values are computed as in Algorithm 1 anddenote the horizontal components of the error matrix M .
4386
Fig. 2. A local map of the environment along with traces from thesatellites to the robot. Green rays indicate free signal paths, orangerays indicate obstructed signal paths.
III. Improved Sensor Model
The improved sensor model is computed from non-occluded satellites, where occlusions are detected by raytracing on a local 3D map of the environment. Thissection describes the generation of local 3D maps, com-putation of the new HDOP model, and filtering of thecovariance.
A. Map GenerationThe presented method requires 3-dimensional point
clouds in the local coordinate frame of the sensor. Ingeneral, these can be generated by any sensor, such as a3D camera, stereo vision, or a rotating laser range finder,operating at a sufficient level of accuracy. In our imple-mentation, a rotating laser range finder has been used(see Section IV). These point clouds are then projectedon the robot coordinate frame and accumulated to forma map of the environment. To preserve local consistencypoint clouds are aligned using the iterative closest point(ICP) algorithm [10]. The projection is obtained as fol-lows. Let P be a point cloud and ξ the 6-dimensionalrobot pose both at time t with ξ = (x, y, z,φ, θ,ψ)T .p ∈ P denotes a point in local coordinates (px, py, pz)T .Let Mx(α) denote the rotation around the robot’s x-axisabout α and analog for My(β) and Mz(γ). The projectionw of p in the robot’s coordinate frame is
wp = (x, y, z)T + Mx(φ) · My(θ) · Mz(ψ) · p . (5)
In an iterative process we obtain W =�
p∈P wp. Figure 2depicts a visualization of a thereby obtained map.
Given the point cloud representation and relative satel-lite locations, one can detect obstructed satellites if linestraced from the receiver towards satellites locations areblocked by objects. Note that relative satellite locationsare computed with respect to the last global pose esti-mate of the robot.
Because ray tracing is a time consuming operation, thelocal map is maintained within a tree-like data structureaccelerating nearest neighbor search. Therefore, point
input : satellite position θ, φ (spherical),GPS antenna position (x, y, z), map M
max ray length rmax, step size rinc
output: true if satellite’s signal path is ob-structed, false otherwise
r = 0 // Distance from antenna1
while r < rmax do2
(dx, dy, dz) ← sphere2cartesian(θ, φ, r)3
q = (qx, qy, qz) ← (x, y, z) + (dx, dy, dz)4
rsearch ← determineSearchRadius(r)5
Find neighbors N of q closer than rsearch6
if signal_path_obstructed(N,r) then7
return true8
r ← r + rinc9
return false10
Algorithm 2: Ray tracing algorithm for detect-ing obstructed signal paths
clouds are stored in ANN-trees (Approximate NearestNeighbor [11]), which are approximated kd-trees allowingto tradeoff query speed and accuracy by a parameter� ∈ R. For � > 0, ANN-trees approximate the nearestneighbor of a point p by a point q whose distance to pexceeds the distance to the true nearest neighbor by afactor of at most (1 + �). The k-nearest neighbors of aquery point can be enumerated in O(k log n), where n isthe numbers of stored points.
B. New HDOP CalculationWith the local map represented as ANN-tree, we can
easily identify blocked signal paths by using ray tracingas in Algorithm 2. The positions of satellites are reportedby conventional GPS receivers in spherical coordinatesθ and φ which are converted to unit vectors using thefunction spere2cartesian. The location of the antennais obtained from the robot’s position plus an offset.The signal path of a GPS satellite is approximated bya set of query points in fixed intervals along a rayfrom the robot to the satellite. Obstruction is detectedusing a fixed-radius neighbor search around the querypoints. The search radius is determined by the functiondetermineSearchRadius which increases with the dis-tance to the robot to compensate for the decreasing res-olution of the LRF. A bounded linear model was chosento this end. After some experiments with the functionsignal_path_obstructed we finally found that a simplethreshold on the number of neighbors yields reasonableresults at the advantage of easy implementation and lowerror-proneness.
Once we know that the signal path of a satellite kis blocked, we remove it from the set of satellites. Theremaining satellites form the set of visible satellites, Svis.We use only the satellites from this set to calculate theuncertainty of a GPS measurement by applying Algo-rithm 1 to Svis. Thereby, we obtain Mvis and HDOPvis.These values can only be calculated, if |Svis| > 3.Otherwise, we set the uncertainty to a high value Rinf .
4387
The obtained values indicate how desirable the visiblesatellites are distributed over the celestial sphere fora GPS position estimate. The UKF requires an arealmeasure of the uncertainty. Therefore, we define theuncertainty of the GPS measurements as
RImp = (Mvis) 2×2 · σ2
UERE , (6)
where (M)2×2 denotes the upper left 2× 2 submatrix ofmatrix M . The standard deviation σUERE was experi-mentally determined as 3.75 m from collecting station-ary measurements with good satellite constellation (lowHDOP values) and free of multipath effects.
C. Covariance FilteringAccording to Equation 6 covariance RImp grows as
more as GPS signals are shadowed from multipath ef-fects, and vice versa. However, due to an incompletemapping of the environment or larger vibrations of the3D sensor, the structure of RImp can unsteadily change,e.g., causing a series of larger covariances interrupted bya series of smaller ones. This is clearly an undesired ef-fect since in such situations overconfident measurementsmight be fused by the filter perturbing the pose esti-mate. Furthermore, we observed that receivers requirerecover time after they have been exposed to multipathsituations. Hence, rapid changes in the covariance matrixhave to be avoided, which is achieved by Algorithm 3.
In Algorithm 3 we apply two different thresholds,
input : GPS uncertainty R
output: improved GPS uncertainty R�
classification GPSgood, GPSbad
σp ←p
R1,1 + R2,2 // ≡ σUERE hDOPvis1
R� ← R2
R�(σp > σthresh)← Rinf // threshold R3
foreach interval iv with σp > σsmooth do4
i← iv(end) + 1 // apply smoothing5
len← min(length(iv) · lenscale, lenmax)6
R�(i : i + len)← Rinf // to successors7
GPSbad ← find(R� = Rinf )8
GPSgood ← find(R�< Rinf )9
return R�
10
Algorithm 3: Filtering and inflation of R yield-ing R� and classification GPSgood, GPSbad
where σthresh is the maximally accepted standard de-viation of the GPS position error. If the error exceedsthis value we set the uncertainty to a very high valueRinf to give the corresponding measurements virtuallyno weight in the filter. A second threshold affectingthe standard deviation σp is σsmooth, which activatessmoothing. When observing a series of measurementswith high uncertainties (exceeding σsmooth), it is evidentthat the receiver is within a error-prone location. Then,uncertainties of the successor values are inflated to obtainsmoothed uncertainties, and to respect the recovery time
of the receiver. The amount of smoothing depends on thenumber of measurements with errors exceeding σsmooth.During our experiments we set Rinf = diag(6002, 6002),σthresh = 6, σsmooth = 9, lenscale = 0.2, lenmax = 12.These values were experimentally determined on trainingdata.
The result of Algorithm 3 can be seen as binaryclassification. GPS measurements with an R�
Imp value ofRinf are given virtually no weight in the state estimationprocess while the remainder of the measurements areconsidered as usable. To evaluate whether we inflated theright measurements we form two classes GPSgood andGPSbad. In the experiments, we compare this classifica-tion to a reference classification based on the observederror.
IV. Experimental Results
Experiments presented in this section have beenconducted with the all-terrain robot telemax fromthe telerob [12] company, designed for bomb disposaland reconnaissance missions. The robot is additionallyequipped with a SICK LMS 200 scanner measuringat a field of view of 180◦ and a range of 80 m. Thescanner is continuously rotated by a PowerCube devicefrom Schunk to obtain 3D scans from the environment.The PowerCube rotates the LRF at constant rate of90◦/s around an axis parallel to the ground. Due to thehigh accuracy of the PowerCube device it is possible togenerate point clouds by accumulating multiple rangereadings with respect to their corresponding scan andPowerCube angle. Accumulated scans from a half rota-tion of the PowerCube form a hemisphere with a radius of80 m. As already mentioned above, point clouds taken atdifferent time steps are consistently registered by the ICPalgorithm taking odometry and IMU bearing as an initialguess. Odometry is obtained from the wheel encodersattached to the tracks of the robot. As IMU sensoran Crossbow AHRS440 has been used which providesabsolute heading estimates by fusing the measurementsof the integrated magnetometers, accelerometers and gy-roscopes. The telemax robot with sensor setup is shownin Figure 3 (a), and the PowerCube with laser scannerin Figure 3 (b).
Data was collected from odometry, IMU, the LRF, andthe GPS module. The robot was manually controlledwith a wireless controller. Most of the time, the robotwas set to full speed which is ≈ 1 m/s. With this setupwe recorded three logfiles which we refer to as Log 014,Log 022, and Log 042. In a time consuming process, wecreated ground truth by manually aligning data from ahigh-precision D-GPS receiver (Trimble GPS PathfinderProXT) with aerial images and the maps created fromthe LRF.
A. ClassificationIn the first experiment we evaluated the algorithm’s
ability to identify unusable GPS measurements caused by
4388
(a) (b)
Fig. 3. (a) Telemax robot approaching a building entrance in anurban environment. (b) Rotating device mounted on the robot fortaking 3D scans online during navigation.
Instances Log 014 Log 042 Log 022
Class. corr. 1178 79.3% 507 73.0% 1708 78.7%
Class. incor. 308 20.7% 188 27.0% 463 21.3%
#GPS Good 1102 538 1578
#GPS Bad 384 157 593
Total 1486 695 2171
Estimated as Estimated as Estimated as
True Class Good Bad Good Bad Good Bad
GPS Good 0.79 0.21 0.69 0.31 0.84 0.16
GPS Bad 0.21 0.79 0.14 0.86 0.36 0.64
TABLE I
Classification results including confusion matrix.
multipath effects. Therefore, we compared on the threelogfiles the output of our algorithms with a reference clas-sification. The reference classification was obtained bycomputing the distance to the ground truth distGPS forevery GPS measurement. A measurement was consideredunusable for navigational purpose if distGPS exceededthe threshold σthresh, which we set to 6 m.
Figure 4 illustrates the estimated GPS uncertainty forLog 014 for the conventional uncertainty model (σGPS),and for the improved model (σ�Improved) along with thetrue GPS error distGPS . The improved model clearlypredicts the true GPS error more accurately than thedefault model. Figure 5 visualizes the classification weobtained for Log 042 and Log 022. GPS measurementsassigned to GPSbad by our algorithm are marked in red.All remaining measurements are marked in green. Onecan clearly see how the GPS measurements drift awayfrom the yellow line indicating the ground truth. Ourapproach correctly identified those bad measurements.Once they converged back towards ground truth theywere identified as good measurements again.
The comparison of the reference classification withthe classification from our algorithm is summarized inTable I. We achieved a correct assignment for 70−80 % ofthe GPS measurements. The algorithm was able to iden-tify at least 64 % of the unusable GPS measurements. InFigure 5 the robot traveled from our campus (right) to
0 200 400 600 800 1000 1200 1400
10
20
30
Time in s
Dis
tanc
ein
m
σGPS
distGPS
σ�Improved
Fig. 4. Comparison of GPS error and uncertainties for Log 014.The improved model σ�Improved predicts the true error better thanthe conventional model σGPS . The σ�Improved has been cropped at20 m.
(a) Log 042 Overview and Details
(b) Log 022 Overview and Details
Fig. 5. GPS uncertainty classification. Red dots indicate GPSmeasurements classified as unusable for localization, whereas greendots indicate good measurements. The true robot trajectory isdrawn in yellow.
the university hospital’s campus (left) and back again.During this experiment the robot traveled very closeto buildings leading to GPS errors of up to 30 m dueto multipath. Our method successfully identified thesecritical situations as shown in the figure.
B. Pose Error
In this section, we present an evaluation of applyingthe improved sensor model with an UKF filter. Wecompare the results from the conventional GPS sensormodel with the results from the improved sensor model.Figure 6 shows the results from applying the filteringon the three logfiles. One situation worth mentioning isthe difference in the position estimate for the multipathsituation near the buildings on the upper right in Figure 6(a) and (b). The robot drove on the pavement next to thetop-most building in a counter-clockwise fashion. Posetracking with the improved model (cyan line) approx-
4389
(c)
(a)
(b)
Fig. 6. Results from the UKF. Cyan lines visualize the results withthe improved sensor model, blue lines with the conventional model.The true Robot trajectory is drawn in yellow.
Error Odometry Conv. Model Impr. Model Difference
Log 014Position
avg. 31.05± 17.49 7.09± 9.19 4.36± 3.76 38.5 %
max. 95.29 39.06 19.01
Orientation
avg. 0.35± 0.37 0.29± 0.35 0.21± 0.30 27.6 %
Log 042Position
avg. 55.24± 30.10 4.63± 4.98 2.90± 1.74 37.4 %
max. 116.90 22.21 10.70
Orientation
avg. 0.47± 0.53 0.34± 0.65 0.30± 0.64 11.8 %
Log 022Position
avg. 53.10± 34.14 5.90± 7.76 4.65± 4.77 21.2 %
max. 116.90 37.85 37.58
Orientation
avg. 0.28± 0.42 0.28± 0.45 0.25± 0.45 10.7 %
TABLE II
Errors for pose estimation. Units are m for positional
errors and radian for orientational errors.
imated the actual trajectory of the robot comparablybetter than the tracking with the conventional modeldiverging up to 40 m away from the actual trajectory.
The results of the filtering are summarized in Table II.Most important to note is that the average and maximumerrors in the position estimate as well as the standarddeviation are significantly reduced by using the improvedsensor model compared to the conventional model. Theimprovement of the average Euclidean distance errorwas between 21− 38 %, and the improvement of theorientation estimate between 10− 27 %.
V. Conclusion
In this paper we presented a method of computingthe uncertainty of GPS measurements with special at-tention to the multipath effect. The method is based on
computing DOP values from only those satellites whosesignal path is not obstructed. Obstruction of the signalpath is determined using a 3D representation of the localenvironment which is generated online. The method wasimplemented and tested in realistic environments on theall-terrain robot platform telemax. Experimental resultshave shown that the improved sensor model outperformsthe conventional model particular for tasks where mobilerobots have to navigate close to buildings. We expecteven better results when high precision IMUs, e.g., withfiber optics gyro, are used. In this case, an overconfidentGPS error model has an even stronger impact on theresulting trajectory.
One difficulty we experienced while conducting exper-iments is the influence of larger trees. They are detectedcorrectly as objects in the scan. However, their influenceon multipath effects seems to be less significant thanthe one of buildings. One solution to this problem is todistinguish trees from building structures and to treatthem differently in the sensor model.
It seems to be obvious that there can be further im-provements when utilizing the information about shad-owed satellites on the receiver level directly, i.e., usingpseudoranges only from satellites that are in line-of-sight for computing the GPS position. In future workwe will consider to extend the approach towards sucha tightly coupled Kalman filter, although it might thenbe applicable only to a few receivers which provide thenecessary pseudorange information.
References
[1] E. Abbott and D. Powell, “Land-vehicle navigation using gps,”Proceedings of the IEEE, vol. 87, no. 1, pp. 145–162, Jan 1999.
[2] P. Goel, et al., “Robust localization using relative and absoluteposition estimates,” in Intelligent Robots and Systems, 1999.
IROS ’99. Proceedings. 1999 IEEE/RSJ International Con-
ference on, vol. 2, 1999, pp. 1134–1140 vol.2.[3] R. Thrapp, et al., “Robust localization algorithms for an
autonomous campus tour guide,” in ICRA. IEEE, 2001, pp.2065–2071.
[4] F. Caron, et al., “Gps/imu data fusion using multisensorkalman filtering: introduction of contextual aspects,” Inf. Fu-
sion, vol. 7, no. 2, pp. 221–230, 2006.[5] A. Giremus, et al., “A particle filtering approach for joint de-
tection/estimation of multipath effects on gps measurements,”Signal Processing, IEEE Transactions on, vol. 55, no. 4, pp.1275–1285, April 2007.
[6] M. Brenneman, et al., “An anova-based gps multipath detec-tion algorithm using multi-channel software receivers,” in Po-
sition, Location and Navigation Symposium, 2008 IEEE/ION,May 2008, pp. 1316–1323.
[7] “DSTA TechX challenge,” 2008, available: www.dsta.gov.sg/index.php/TechX-Challenge/.
[8] S. J. Julier and J. K. Uhlmann,“A new extension of the kalmanfilter to nonlinear systems,” in Int. Symp. Aerospace/Defense
Sensing, Simul. and Controls, 1997, pp. 182–193.[9] B. W. Parkinson and J. J. Spilker, Global Positioning System:
Theory and Applications. The American Institute of Aero-nautics and Astronautics, 1996, vol. 1, ch. GPS Error Analysis.
[10] P. J. Besl and N. D. McKay, “A method for registration of 3-d shapes,” IEEE Trans. Pattern Anal. Mach. Intell., vol. 14,no. 2, pp. 239–256, 1992.
[11] S. Arya, et al., “An optimal algorithm for approximate nearestneighbor searching,” J. ACM, vol. 45, pp. 891–923, 1998.
[12] Telerob Gesellschaft fur Fernhantierungstechnik mbH, avail-able: http://www.telerob.de.
4390