Trilateration - Today at Minesinside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf ·...
Transcript of Trilateration - Today at Minesinside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf ·...
![Page 1: Trilateration - Today at Minesinside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf · Acknowledgements William Murphy(M.S. Dissertation, CSM, 1992) William Navidi(Department](https://reader034.fdocuments.net/reader034/viewer/2022052012/6028d428e0e17a261e363cc2/html5/thumbnails/1.jpg)
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.
http://inside.mines.edu/∼whereman/
Department of Computer Engineering
Turgut Ozal University, Kecioren, Ankara, Turkey
Tuesday, June 21, 2011, 11:00
![Page 2: Trilateration - Today at Minesinside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf · Acknowledgements William Murphy(M.S. Dissertation, CSM, 1992) William Navidi(Department](https://reader034.fdocuments.net/reader034/viewer/2022052012/6028d428e0e17a261e363cc2/html5/thumbnails/2.jpg)
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
![Page 3: Trilateration - Today at Minesinside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf · Acknowledgements William Murphy(M.S. Dissertation, CSM, 1992) William Navidi(Department](https://reader034.fdocuments.net/reader034/viewer/2022052012/6028d428e0e17a261e363cc2/html5/thumbnails/3.jpg)
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
![Page 4: Trilateration - Today at Minesinside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf · Acknowledgements William Murphy(M.S. Dissertation, CSM, 1992) William Navidi(Department](https://reader034.fdocuments.net/reader034/viewer/2022052012/6028d428e0e17a261e363cc2/html5/thumbnails/4.jpg)
Global Positioning System (GPS)
![Page 5: Trilateration - Today at Minesinside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf · Acknowledgements William Murphy(M.S. Dissertation, CSM, 1992) William Navidi(Department](https://reader034.fdocuments.net/reader034/viewer/2022052012/6028d428e0e17a261e363cc2/html5/thumbnails/5.jpg)
![Page 6: Trilateration - Today at Minesinside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf · Acknowledgements William Murphy(M.S. Dissertation, CSM, 1992) William Navidi(Department](https://reader034.fdocuments.net/reader034/viewer/2022052012/6028d428e0e17a261e363cc2/html5/thumbnails/6.jpg)
How did we get into this?
![Page 7: Trilateration - Today at Minesinside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf · Acknowledgements William Murphy(M.S. Dissertation, CSM, 1992) William Navidi(Department](https://reader034.fdocuments.net/reader034/viewer/2022052012/6028d428e0e17a261e363cc2/html5/thumbnails/7.jpg)
Accidents Happen
![Page 8: Trilateration - Today at Minesinside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf · Acknowledgements William Murphy(M.S. Dissertation, CSM, 1992) William Navidi(Department](https://reader034.fdocuments.net/reader034/viewer/2022052012/6028d428e0e17a261e363cc2/html5/thumbnails/8.jpg)
Bulldozer with Beacons
![Page 9: Trilateration - Today at Minesinside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf · Acknowledgements William Murphy(M.S. Dissertation, CSM, 1992) William Navidi(Department](https://reader034.fdocuments.net/reader034/viewer/2022052012/6028d428e0e17a261e363cc2/html5/thumbnails/9.jpg)
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
![Page 10: Trilateration - Today at Minesinside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf · Acknowledgements William Murphy(M.S. Dissertation, CSM, 1992) William Navidi(Department](https://reader034.fdocuments.net/reader034/viewer/2022052012/6028d428e0e17a261e363cc2/html5/thumbnails/10.jpg)
![Page 11: Trilateration - Today at Minesinside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf · Acknowledgements William Murphy(M.S. Dissertation, CSM, 1992) William Navidi(Department](https://reader034.fdocuments.net/reader034/viewer/2022052012/6028d428e0e17a261e363cc2/html5/thumbnails/11.jpg)
![Page 12: Trilateration - Today at Minesinside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf · Acknowledgements William Murphy(M.S. Dissertation, CSM, 1992) William Navidi(Department](https://reader034.fdocuments.net/reader034/viewer/2022052012/6028d428e0e17a261e363cc2/html5/thumbnails/12.jpg)
Problem Statement and Setup
![Page 13: Trilateration - Today at Minesinside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf · Acknowledgements William Murphy(M.S. Dissertation, CSM, 1992) William Navidi(Department](https://reader034.fdocuments.net/reader034/viewer/2022052012/6028d428e0e17a261e363cc2/html5/thumbnails/13.jpg)
Notations
θ = (x, y, z) : spatial coordinates of target point θ.
Bi = (xi, yi, zi) : exact location of beacon Bi.
i = 1, 2, . . . , n with n ≥ 4.
di(θ) =√
(x− xi)2 + (y − yi)2 + (z − zi)2 : true distance
between beacon Bi and target θ.
(xr, yr, zr) : exact coordinates of a reference point.
dir =√
(xi − xr)2 + (yi − yr)2 + (zi − zr)2 : true distance
between reference point and beacon Bi.
dr(θ) =√
(x− xr)2 + (y − yr)2 + (z − zr)2 : true distance
between the reference point and the target θ.
![Page 14: Trilateration - Today at Minesinside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf · Acknowledgements William Murphy(M.S. Dissertation, CSM, 1992) William Navidi(Department](https://reader034.fdocuments.net/reader034/viewer/2022052012/6028d428e0e17a261e363cc2/html5/thumbnails/14.jpg)
Derivation of an Exact Linear Model
Apply a simple trick (the cosine rule!)
di(θ)2 = (x− xi)2 + (y − yi)2 + (z − zi)2
= (x− xr + xr − xi)2 + (y − yr + yr − yi)2
+(z − zr + zr − zi)2
= (x− xr)2 + 2 (xr − xi)(x− xr) + (xr − xi)2
+(y − yr)2 + 2 (yr − yi)(y − yr) + (yr − yi)2
+(z − zr)2 + 2 (zr − zi)(z − zr) + (zr − zi)2
![Page 15: Trilateration - Today at Minesinside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf · Acknowledgements William Murphy(M.S. Dissertation, CSM, 1992) William Navidi(Department](https://reader034.fdocuments.net/reader034/viewer/2022052012/6028d428e0e17a261e363cc2/html5/thumbnails/15.jpg)
Keep the double product terms on the left hand side.
2((xi − xr)(x− xr) + (yi − yr)(y − yr) + (zi − zr)(z − zr)
)= (x− xr)2 + (y − yr)2 + (z − zr)2
+(xr − xi)2 + (yr − yi)2 + (zr − zi)2 − di(θ)2
= dr(θ)2 + dir2 − di(θ)2
where i = 1, 2, . . . , n with n ≥ 4.
![Page 16: Trilateration - Today at Minesinside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf · Acknowledgements William Murphy(M.S. Dissertation, CSM, 1992) William Navidi(Department](https://reader034.fdocuments.net/reader034/viewer/2022052012/6028d428e0e17a261e363cc2/html5/thumbnails/16.jpg)
Use any beacon (say, B1) as reference point.
Replace exact distances by measured distances.
(x2 − x1)(x− x1) + (y2 − y1)(y − y1) + (z2 − z1)(z − z1)
≈1
2
[r1
2 − r22 + d221
]:= b21
(x3 − x1)(x− x1) + (y3 − y1)(y − y1) + (z3 − z1)(z − z1)
≈1
2
[r1
2 − r32 + d231
]:= b31
...
(xn − x1)(x− x1) + (yn − y1)(y − y1) + (zn − z1)(z − z1)
≈1
2
[r1
2 − rn2 + d2n1
]:= bn1
Linear system of (n− 1) equations in 3 unknowns.
![Page 17: Trilateration - Today at Minesinside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf · Acknowledgements William Murphy(M.S. Dissertation, CSM, 1992) William Navidi(Department](https://reader034.fdocuments.net/reader034/viewer/2022052012/6028d428e0e17a261e363cc2/html5/thumbnails/17.jpg)
Linear Least Squares (LSQ) Model
Write the linear system in matrix form: Ax ≈ b
with
A=
x2−x1 y2−y1 z2−z1x3−x1 y3−y1 z3−z1
......
...
xn−x1 yn−y1 zn−z1
, x=
x−x1
y−y1
z−z1
, b=
b21
b31
...
bn1
![Page 18: Trilateration - Today at Minesinside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf · Acknowledgements William Murphy(M.S. Dissertation, CSM, 1992) William Navidi(Department](https://reader034.fdocuments.net/reader034/viewer/2022052012/6028d428e0e17a261e363cc2/html5/thumbnails/18.jpg)
Minimizing the sum of the squares of the residuals
S = (b−Ax)T(b−Ax)
requires solving the normal equation
ATAx = ATb
Solution method depends on the condition number ofATA.
If ATA is non-singular and well-conditioned then
x = (ATA)−1ATb
![Page 19: Trilateration - Today at Minesinside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf · Acknowledgements William Murphy(M.S. Dissertation, CSM, 1992) William Navidi(Department](https://reader034.fdocuments.net/reader034/viewer/2022052012/6028d428e0e17a261e363cc2/html5/thumbnails/19.jpg)
If ATA is nearly-singular (poorly conditioned):
? Compute A = QR
Q is orthonormal matrix,
R is upper-triangular matrix.
? Solve Rx = QT~b
by back substitution when A is full rank.
The target θ is then θ =
x
y
z
= x +
x1
y1
z1
.
![Page 20: Trilateration - Today at Minesinside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf · Acknowledgements William Murphy(M.S. Dissertation, CSM, 1992) William Navidi(Department](https://reader034.fdocuments.net/reader034/viewer/2022052012/6028d428e0e17a261e363cc2/html5/thumbnails/20.jpg)
Nonlinear Least Squares (NLSQ) Model
Minimize the sum of the squares of the errors on the
distances:
F (θ) = F (x, y, z) =
n∑i=1
fi(x, y, z)2
where
fi(x, y, z) = fi(θ) := di(θ)− ri=√
(x− xi)2 + (y − yi)2 + (z − zi)2 − ri.
Recall: ri are the measured distances between thetarget θ = (x, y, z) and beacon Bi = (xi, yi, zi), and n is
the number of beacons.
![Page 21: Trilateration - Today at Minesinside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf · Acknowledgements William Murphy(M.S. Dissertation, CSM, 1992) William Navidi(Department](https://reader034.fdocuments.net/reader034/viewer/2022052012/6028d428e0e17a261e363cc2/html5/thumbnails/21.jpg)
Differentiating F with respect to x yields
∂F (θ)
∂x= 2
n∑i=1
fi∂fi(θ)
∂x= 2
n∑i=1
fi∂di(θ)
∂x.
The formulae for ∂F (θ)∂y
and ∂F (θ)∂z
are similar.
Let
f(θ) =
f1(θ)
f2(θ)...
fn(θ)
, ∇F (θ) =
∂F (θ)∂x
∂F (θ)∂y
∂F (θ)∂z
![Page 22: Trilateration - Today at Minesinside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf · Acknowledgements William Murphy(M.S. Dissertation, CSM, 1992) William Navidi(Department](https://reader034.fdocuments.net/reader034/viewer/2022052012/6028d428e0e17a261e363cc2/html5/thumbnails/22.jpg)
and define the Jacobian as
J(θ) =
∂d1(θ)∂x
∂d1(θ)∂y
∂d1(θ)∂z
∂d2(θ)∂x
∂d2(θ)∂y
∂d2(θ)∂z
......
...
∂dn(θ)∂x
∂dn(θ)∂y
∂dn(θ)∂z
![Page 23: Trilateration - Today at Minesinside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf · Acknowledgements William Murphy(M.S. Dissertation, CSM, 1992) William Navidi(Department](https://reader034.fdocuments.net/reader034/viewer/2022052012/6028d428e0e17a261e363cc2/html5/thumbnails/23.jpg)
We must solve
∇F (θ) = 2J(θ)Tf(θ) = 0
where
J(θ)Tf(θ) =
∑ni=1
(x−xi)fi(θ)di(θ)
∑ni=1
(y−yi)fi(θ)di(θ)
∑ni=1
(z−zi)fi(θ)di(θ)
.
![Page 24: Trilateration - Today at Minesinside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf · Acknowledgements William Murphy(M.S. Dissertation, CSM, 1992) William Navidi(Department](https://reader034.fdocuments.net/reader034/viewer/2022052012/6028d428e0e17a261e363cc2/html5/thumbnails/24.jpg)
Newton-Raphson Method – Iterative Solver
Problem: Solve the scalar problem f(x) = 0
Solution: Newton’s method:
xk+1 = xk −f(xk)
f ′(xk)
Problem: Solve the vector problem: f(x) = 0
simplest case: n equations, n unknowns.
Solution: Newton’s method:
xk+1 = xk − [J(xk)]−1f(xk)
![Page 25: Trilateration - Today at Minesinside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf · Acknowledgements William Murphy(M.S. Dissertation, CSM, 1992) William Navidi(Department](https://reader034.fdocuments.net/reader034/viewer/2022052012/6028d428e0e17a261e363cc2/html5/thumbnails/25.jpg)
Apply Newton’s method to g(θ) = J(θ)Tf(θ) = 0.
Solution:
θ{k+1} = θ{k} − [J(θ{k})TJ(θ{k})]
−1J(θ{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.
![Page 26: Trilateration - Today at Minesinside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf · Acknowledgements William Murphy(M.S. Dissertation, CSM, 1992) William Navidi(Department](https://reader034.fdocuments.net/reader034/viewer/2022052012/6028d428e0e17a261e363cc2/html5/thumbnails/26.jpg)
The expression for J(θ)TJ(θ) is
∑ni=1
(x−xi)2
di(θ)2
∑ni=1
(x−xi)(y−yi)
di(θ)2
∑ni=1
(x−xi)(z−zi)
di(θ)2
∑ni=1
(x−xi)(y−yi)
di(θ)2
∑ni=1
(y−yi)2
di(θ)2
∑ni=1
(y−yi)(z−zi)
di(θ)2
∑ni=1
(x−xi)(z−zi)
di(θ)2
∑ni=1
(y−yi)(z−zi)
di(θ)2
∑ni=1
(z−zi)2
di(θ)2
.
![Page 27: Trilateration - Today at Minesinside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf · Acknowledgements William Murphy(M.S. Dissertation, CSM, 1992) William Navidi(Department](https://reader034.fdocuments.net/reader034/viewer/2022052012/6028d428e0e17a261e363cc2/html5/thumbnails/27.jpg)
Mathematica Demonstration 1
Computation of Target using the NLSQ Method
Mathematica’s NMinimize Function
![Page 28: Trilateration - Today at Minesinside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf · Acknowledgements William Murphy(M.S. Dissertation, CSM, 1992) William Navidi(Department](https://reader034.fdocuments.net/reader034/viewer/2022052012/6028d428e0e17a261e363cc2/html5/thumbnails/28.jpg)
Mathematica Demonstration 2
Computation of Target using the NLSQ Method
Newton Iteration
![Page 29: Trilateration - Today at Minesinside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf · Acknowledgements William Murphy(M.S. Dissertation, CSM, 1992) William Navidi(Department](https://reader034.fdocuments.net/reader034/viewer/2022052012/6028d428e0e17a261e363cc2/html5/thumbnails/29.jpg)
Mathematica Demonstration 3
Computation of Target using the LSQ Method
![Page 30: Trilateration - Today at Minesinside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf · Acknowledgements William Murphy(M.S. Dissertation, CSM, 1992) William Navidi(Department](https://reader034.fdocuments.net/reader034/viewer/2022052012/6028d428e0e17a261e363cc2/html5/thumbnails/30.jpg)
Simulation – Results of Experiments
Beacon coordinates (8 beacons were used)
X Y Z
920 3977.5 −77.125
7360 2577.5 −53.125
8090 −3892.5 83.875
3910 −4512.5 27.875
−2710 −3742.5 4.875
−5420 −1082.5 55.875
−6740 1657.5 −42.125
−5410 5017.5 −0.125
![Page 31: Trilateration - Today at Minesinside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf · Acknowledgements William Murphy(M.S. Dissertation, CSM, 1992) William Navidi(Department](https://reader034.fdocuments.net/reader034/viewer/2022052012/6028d428e0e17a261e363cc2/html5/thumbnails/31.jpg)
Location of 8 Beacons
![Page 32: Trilateration - Today at Minesinside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf · Acknowledgements William Murphy(M.S. Dissertation, CSM, 1992) William Navidi(Department](https://reader034.fdocuments.net/reader034/viewer/2022052012/6028d428e0e17a261e363cc2/html5/thumbnails/32.jpg)
Test Grid of 1000 Points
![Page 33: Trilateration - Today at Minesinside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf · Acknowledgements William Murphy(M.S. Dissertation, CSM, 1992) William Navidi(Department](https://reader034.fdocuments.net/reader034/viewer/2022052012/6028d428e0e17a261e363cc2/html5/thumbnails/33.jpg)
• Requirement: determine target within 2 feet
(distances measured within 12
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).
![Page 34: Trilateration - Today at Minesinside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf · Acknowledgements William Murphy(M.S. Dissertation, CSM, 1992) William Navidi(Department](https://reader034.fdocuments.net/reader034/viewer/2022052012/6028d428e0e17a261e363cc2/html5/thumbnails/34.jpg)
• 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).
![Page 35: Trilateration - Today at Minesinside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf · Acknowledgements William Murphy(M.S. Dissertation, CSM, 1992) William Navidi(Department](https://reader034.fdocuments.net/reader034/viewer/2022052012/6028d428e0e17a261e363cc2/html5/thumbnails/35.jpg)
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: http://inside.mines.edu/∼whereman/
![Page 36: Trilateration - Today at Minesinside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf · Acknowledgements William Murphy(M.S. Dissertation, CSM, 1992) William Navidi(Department](https://reader034.fdocuments.net/reader034/viewer/2022052012/6028d428e0e17a261e363cc2/html5/thumbnails/36.jpg)
Thank You
![Page 37: Trilateration - Today at Minesinside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf · Acknowledgements William Murphy(M.S. Dissertation, CSM, 1992) William Navidi(Department](https://reader034.fdocuments.net/reader034/viewer/2022052012/6028d428e0e17a261e363cc2/html5/thumbnails/37.jpg)
Publications
1. W. Navidi, W. Murphy, Jr., and W. Hereman,
Statistical methods in surveying by trilateration,
Computational Statistics and Data Analysis,
27(2), pp. 209-227 (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.