Page 133 - Autonomous Mobile Robots

116 Autonomous Mobile Robots

and

∂ρ i −(X i − x)
∂x = (X i − x) + (Y i − y) + (Z i − z) 2
2
2
∂ρ i −(Y i − y)
∂y = (X i − x) + (Y i − y) + (Z i − z) 2
2
2
∂ρ i −(Z i − z)
∂z = (X i − x) + (Y i − y) + (Z i − z) 2
2
2
Given m simultaneous range measurements, all the measurements can be put
in the matrix form

δρ = Hδx + v (3.46)

by making the definitions

   
ρ 1 h 1 ,1
ρ 2
   h 2 ,1 
and (3.47)
   
δρ =  .  H =  . 
. .  . . 
 
ρ m h m ,1
where the residual measurement is
ρ i =˜ρ i (x) −ˆρ i (ˆ x)

and v represents the high order terms (h.o.t.s) of the linearization plus the
measurement noise.
To determine the state vector, a minimum of four simultaneous range meas-
urements are required. The weighted least squares solution to Equation (3.46) is
T
T
δx =[H R −1 H] −1 H R −1 δρ (3.48)
The corrected position estimate is then

+
ˆ x = ˆ x + δx (3.49)
To reduce the effects of the linearization error terms, the above process can
be repeated using the same measurement data and the corrected position at
the end of the current iteration as the starting point of the next iteration (i.e.,
+
ˆ x = ˆ x ). The iteration is stopped when the error state vector δx converges to
a sufficiently small value. Even after the convergence of δx has been achieved,

© 2006 by Taylor & Francis Group, LLC

FRANKL: “dk6033_c003” — 2006/3/31 — 16:42 — page 116 — #18