Page 135 - Autonomous Mobile Robots

118 Autonomous Mobile Robots

To find the position corresponding to range measurements in the 2D
example, we define the position and clock bias error vector as

ˆ
δp = x − ˆ x =[x −ˆx, y −ˆy, bˆu − b u ] T
The range is computed as

2 2 ˆ
ˆ ρ i (ˆ x) = (X i −ˆx) + (Y i −ˆy) + b u
The line-of-sight vector (from satellite to user) is

−(X i −ˆx) −(Y i −ˆy)
h i = , (3.52)
2
2
(X i −ˆx) + (Y i −ˆy) 2 (X i −ˆx) + (Y i −ˆy) 2
Because there are three unknowns, measurements from at least three satel-
lites will be required. Let us assume that there are satellites at locations

◦
◦
◦
◦
P i = 10 × 10 6 sin θ i m for θ 1 = 90 , θ 2 = 85 , θ 3 = 20 , and θ 4 =−85 with
cos θ i
corresponding range measurements of ρ 1 = 9.513151e6, ρ 2 = 9.469241e6,
ρ 3 = 9.363915e6, and ρ 4 = 10.468545e6. Then, if the initial position estimate
T
is ˆ x = [0.00, 0.00, 0.00 ] , the sequence of positions and position corrections
computed by iterating Equation(3.48)and Equation (3.49)with R = I, isshown
in Table 3.1. Note that if the initial estimate, possibly obtained by propagation
of the estimate from a previous epoch, was accurate to approximately 10 m, then
one or possibly two iterations would provide convergence of a new estimate
consistent with the measurements of the current epoch to better than millimeter
accuracy. Also, even after the estimate of x has converged to micrometer accur-
acy, the error in the estimated measurement is still 0.44 m. This is the least
squared error that can be achieved by adjusting the three elements of x to fit the
four measurements of ρ.

TABLE 3.1
Results of Computations for Example 3.2

Iteration δx δx ˆ x ρ − ˆ ρ
0 NA NA [0, 0, 0]
1 [5.01, 5.09, 0.14]e5 7.1e5 [5.011961, 5.090871, 1.364810]e5 23368.75
2 [−0.12, −0.91, −1.36]e4 1.6e4 [5.000000, 5.000046, 0.000062]e5 7.33
3 [0.01, −4.29, −4.21]e0 6.0e0 [5.000000, 5.000000, 0.000002]e5 0.44
4 [−0.26, −8.53, −9.29]e−7 1.3e−6 [5.000000, 5.000000, 0.000002]e5 0.44

© 2006 by Taylor & Francis Group, LLC

FRANKL: “dk6033_c003” — 2006/3/31 — 16:42 — page 118 — #20