Trilateration: The Mathematics Behind a Local Positioning System. Willy Hereman

Transcription

1 Trilateration: The Mathematics Behind a Local Positioning System Willy Hereman Department of Mathematical and Computer Sciences Colorado School of Mines Golden, Colorado, U.S.A. whereman@mines.edu whereman/ Department of Computer Engineering Turgut Özal University, Keçiören, Ankara, Turkey Tuesday, June 21, 2011, 11:00

2 Acknowledgements William Murphy (M.S. Dissertation, CSM, 1992) William Navidi (Department of MCS, CSM) Atlantic Richfield Company (ARCO) Thunder Basin Coal Company, Wright, Wyoming This presentation was made in TeXpower

3 Outline Problem statement: What is trilateration? How did we get into this? Applications of our algorithm An exact linearization Linear least squares method Nonlinear least squares method Mathematica demonstration Simulation Results of experiments Conclusions

4 Global Positioning System (GPS)

5

6 How did we get into this?

7 Accidents Happen

8 Bulldozer with Beacons

9 Applications of our Algorithm Thunder Basin Coal Mine locating bulldozers Surveying without triangulation (Mining) Mobile computing sensor networks Geosensing networks (SmartGeo) Precision manufacturing Positioning systems for medical applications (Electrical Engineering) Ignite program blasting rockets

10

11

12 Problem Statement and Setup

13 Notations θ = (x, y, z) : spatial coordinates of target point θ. B i = (x i, y i, z i ) : exact location of beacon B i. i = 1, 2,..., n with n 4. d i (θ) = (x x i ) 2 + (y y i ) 2 + (z z i ) 2 : true distance between beacon B i and target θ. (x r, y r, z r ) : exact coordinates of a reference point. d ir = (x i x r ) 2 + (y i y r ) 2 + (z i z r ) 2 : true distance between reference point and beacon B i. d r (θ) = (x x r ) 2 + (y y r ) 2 + (z z r ) 2 : true distance between the reference point and the target θ.

14 Derivation of an Exact Linear Model Apply a simple trick (the cosine rule!) d i (θ) 2 = (x x i ) 2 + (y y i ) 2 + (z z i ) 2 = (x x r + x r x i ) 2 + (y y r + y r y i ) 2 +(z z r + z r z i ) 2 = (x x r ) (x r x i )(x x r ) + (x r x i ) 2 +(y y r ) (y r y i )(y y r ) + (y r y i ) 2 +(z z r ) (z r z i )(z z r ) + (z r z i ) 2

15 Keep the double product terms on the left hand side. ( ) 2 (x i x r )(x x r ) + (y i y r )(y y r ) + (z i z r )(z z r ) = (x x r ) 2 + (y y r ) 2 + (z z r ) 2 +(x r x i ) 2 + (y r y i ) 2 + (z r z i ) 2 d i (θ) 2 = d r (θ) 2 + d ir 2 d i (θ) 2 where i = 1, 2,..., n with n 4.

16 Use any beacon (say, B 1 ) as reference point. Replace exact distances by measured distances. (x 2 x 1 )(x x 1 ) + (y 2 y 1 )(y y 1 ) + (z 2 z 1 )(z z 1 ) 2[ 1 2 r1 r d 2 21] := b21 (x 3 x 1 )(x x 1 ) + (y 3 y 1 )(y y 1 ) + (z 3 z 1 )(z z 1 ) 2[ 1 2 r1 r d 2 31] := b31. (x n x 1 )(x x 1 ) + (y n y 1 )(y y 1 ) + (z n z 1 )(z z 1 ) 2[ 1 2 r1 r 2 n + d 2 n1] := bn1 Linear system of (n 1) equations in 3 unknowns.

17 Linear Least Squares (LSQ) Model Write the linear system in matrix form: Ax b with x 2 x 1 y 2 y 1 z 2 z 1 x A= 3 x 1 y 3 y 1 z 3 z 1... x n x 1 y n y 1 z n z 1 x x 1, x= y y 1 z z 1, b= b 21 b 31. b n1

18 Minimizing the sum of the squares of the residuals S = (b Ax) T (b Ax) requires solving the normal equation A T Ax = A T b Solution method depends on the condition number of A T A. If A T A is non-singular and well-conditioned then x = (A T A) 1 A T b

19 If A T A is nearly-singular (poorly conditioned): Compute A = QR Q is orthonormal matrix, R is upper-triangular matrix. Solve Rx = Q T b by back substitution when A is full rank. x x 1 The target θ is then θ = y = x + y 1. z z 1

20 Nonlinear Least Squares (NLSQ) Model Minimize the sum of the squares of the errors on the distances: n F (θ) = F (x, y, z) = f i (x, y, z) 2 where i=1 f i (x, y, z) = f i (θ) := d i (θ) r i = (x x i ) 2 + (y y i ) 2 + (z z i ) 2 r i. Recall: r i are the measured distances between the target θ = (x, y, z) and beacon B i = (x i, y i, z i ), and n is the number of beacons.

21 Differentiating F with respect to x yields F (θ) x = 2 n i=1 f i f i (θ) x = 2 n i=1 f i d i (θ) x. The formulae for Let f(θ) = F (θ) y and F (θ) z f 1 (θ) f 2 (θ), F (θ) =. f n (θ) are similar. F (θ) x F (θ) y F (θ) z

22 and define the Jacobian as J(θ) = d 1 (θ) x d 2 (θ) x. d n (θ) x d 1 (θ) y d 2 (θ) y. d n (θ) y d 1 (θ) z d 2 (θ) z. d n (θ) z

23 We must solve F (θ) = 2J(θ) T f(θ) = 0 where J(θ) T f(θ) = n i=1 n i=1 n i=1 (x x i )f i (θ) d i (θ) (y y i )f i (θ) d i (θ) (z z i )f i (θ) d i (θ).

24 Newton-Raphson Method Iterative Solver Problem: Solve the scalar problem f(x) = 0 Solution: Newton s method: x k+1 = x k f(x k) f (x k ) Problem: Solve the vector problem: f(x) = 0 simplest case: n equations, n unknowns. Solution: Newton s method: x k+1 = x k [J(x k )] 1 f(x k )

25 Apply Newton s method to g(θ) = J(θ) T f(θ) = 0. Solution: θ {k+1} = θ {k} [J(θ {k} ) T J(θ {k} )] 1 J(θ {k} ) T f(θ {k} ) where θ {k} denotes the kth estimate of the target. A reasonably accurate initial guess, θ {1}, could be computed with the LSQ method. Starting with θ {1}, iterate until the change θ {k+1} θ {k} is sufficiently small.

26 The expression for J(θ) T J(θ) is n (x x i ) 2 n (x x i )(y y i ) n (x x i )(z z i ) i=1 d i (θ) 2 i=1 d i (θ) 2 i=1 d i (θ) 2 n (x x i )(y y i ) n (y y i ) 2 n (y y i )(z z i ) i=1 d i (θ) 2 i=1 d i (θ) 2 i=1 d i (θ) 2. n (x x i )(z z i ) n (y y i )(z z i ) n (z z i ) 2 i=1 d i (θ) 2 i=1 d i (θ) 2 i=1 d i (θ) 2

27 Mathematica Demonstration 1 Computation of Target using the NLSQ Method Mathematica s NMinimize Function

28 Mathematica Demonstration 2 Computation of Target using the NLSQ Method Newton Iteration

29 Mathematica Demonstration 3 Computation of Target using the LSQ Method

30 Simulation Results of Experiments Beacon coordinates (8 beacons were used) X Y Z

31 Location of 8 Beacons

32 Test Grid of 1000 Points

33 Requirement: determine target within 2 feet (distances measured within 1 2 foot). One thousand target points on a rectangular grid. Top of box is 5 feet below lowest beacon. For each target point, 10, 000 data sets were generated. Each data set consisted of one measurement from each beacon. Each measurement was obtained by adding to the true distance a random error distributed uniformly on ( 0.5, 0.5).

34 Methods were implemented in Macsyma and C++ Horizontal coordinates were accurate (98% of test points). Vertical coordinate (height) was imprecise (off by several feet for 5% of test points). Trouble with hardware (AccuTrack, Canada).

35 Conclusions Exact linearization for nonlinear problem. LSQ method is reliable even with small samples. NLSQ method gives best performance. Methods are easy to implement. Good alternative for applications where GPS cannot be used. Publications are on the Internet: URL: whereman/

36 Thank You

37 Publications 1. W. Navidi, W. Murphy, Jr., and W. Hereman, Statistical methods in surveying by trilateration, Computational Statistics and Data Analysis, 27(2), pp (1998). 2. W. Murphy and W. Hereman, Determination of a position in three dimensions using trilateration and approximate distances, Technical Report MCS-95-07, Department of Mathematical and Computer Sciences, Colorado School of Mines, Golden, Colorado (1995), 19 pages. 3. W. Murphy, Determination of a Position Using Approximate Distances and Trilateration, M.S. Thesis, Colorado School of Mines, May 1992.