1 /*
2 * Copyright (C) 2018 Alberto Irurueta Carro (alberto@irurueta.com)
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16 package com.irurueta.navigation.lateration;
17
18 import com.irurueta.algebra.AlgebraException;
19 import com.irurueta.algebra.Matrix;
20 import com.irurueta.algebra.Utils;
21 import com.irurueta.geometry.Point;
22 import com.irurueta.navigation.LockedException;
23 import com.irurueta.navigation.NotReadyException;
24
25 /**
26 * Linearly solves the lateration problem using an inhomogeneous solution.
27 * This class is base on the implementation found at:
28 * <a href="https://github.com/lemmingapex/trilateration">https://github.com/lemmingapex/trilateration</a>
29 * Further information and algorithms can be found at Willy Hereman and William S. Murphy Jr. Determination of a
30 * Position in Three Dimensions Using Trilateration and Approximate Distances.
31 *
32 * @param <P> a {@link Point} type.
33 */
34 public abstract class InhomogeneousLinearLeastSquaresLaterationSolver<P extends Point<P>> extends LaterationSolver<P> {
35
36 /**
37 * Constructor.
38 */
39 protected InhomogeneousLinearLeastSquaresLaterationSolver() {
40 super();
41 }
42
43 /**
44 * Constructor.
45 *
46 * @param positions known positions of static nodes.
47 * @param distances euclidean distances from static nodes to mobile node.
48 * @throws IllegalArgumentException if either positions or distances are null, don't have the same length or their
49 * length is smaller than required points.
50 */
51 protected InhomogeneousLinearLeastSquaresLaterationSolver(final P[] positions, final double[] distances) {
52 super(positions, distances);
53 }
54
55 /**
56 * Constructor.
57 *
58 * @param listener listener to be notified of events raised by this instance.
59 */
60 protected InhomogeneousLinearLeastSquaresLaterationSolver(final LaterationSolverListener<P> listener) {
61 super(listener);
62 }
63
64 /**
65 * Constructor.
66 *
67 * @param positions known positions of static nodes.
68 * @param distances euclidean distances from static nodes to mobile node.
69 * @param listener listener to be notified of events raised by this instance.
70 * @throws IllegalArgumentException if either positions or distances are null, don't have the same length or their
71 * length is smaller than required points.
72 */
73 protected InhomogeneousLinearLeastSquaresLaterationSolver(
74 final P[] positions, final double[] distances, final LaterationSolverListener<P> listener) {
75 super(positions, distances, listener);
76 }
77
78 /**
79 * Solves the lateration problem.
80 *
81 * @throws LaterationException if lateration fails.
82 * @throws NotReadyException if solver is not ready.
83 * @throws LockedException if instance is busy solving the lateration problem.
84 */
85 @Override
86 public void solve() throws LaterationException, NotReadyException, LockedException {
87 // The implementation on this method follows the algorithm bellow for 3D but
88 // generalized for both 2D and 3D:
89
90 // The constraints are the equations of the spheres with radii ri,
91 // (x - xi)^2 + (y - yi)^2 + (z - zi)^2 = ri^2 (i = 1,2,...,n)
92 // where (xi, yi, zi) is the center of each sphere and ri is the radius of
93 // each sphere.
94
95 // The jth constraint is used as a linearizing tool (in this implementation we
96 // always used the 1 point as a reference point). Adding and subtracting xj,yj and zj gives
97 // (x - xj + xj - xi)^2 + (y - yj + yj - yi)^2 + (z - zj + zj - zi)^2 = ri^2
98 // with (i = 1,2,...j-1,j+1,...,n).
99
100 // Expanding and regrouping the terms, leads to
101 // (x - xj)*(xi - xj) + (y - yj)*(yi - yj) + (z - zj)*(zi - zj) =
102 // 0.5*((x - xj)^2 + (y - yj)^2 + (z - zj)^2 - ri^2 + (yi - yj)^2 + (zi - zj)^2) =
103 // 0.5*(rj^2 - ri^2 + dij^2) = bij
104 // where
105 // dij = sqrt((xi - xj^2 + (yi - yj^2 + (zi - zj)^2)
106 // is the distance between position i and position j.
107
108 // Since it does not matter which constraint is used as a linearizing tool, arbitrarily select
109 // the first constraint (j = 1). This is analogous to selecting the first position.
110 // Since i = 2,3,...,n, this leads to a linear system of (n - 1) equations in 3 unknowns.
111 // (x - x1)*(x2 - x1) + (y - y1)*(y2 - y1) + (z - z1)*(z2 - z1) = 0.5*(r1^2 - r2^2 + d21^2) = b21
112 // (x - x1)*(x3 - x1) + (y - y1)*(y3 - y1) + (z - z1)*(z3 - z1) = 0.5*(r1^2 - r3^2 + d31^2) = b31
113 // ...
114 // (x - x1)*(xn - x1) + (y - y1)*(yn - y1) + (z - z1)*(zn - z1) = 0.5*(r1^2 - rn^2 + dn1^2) = bn1
115
116 // This linear system is easily written in matrix form A*x = b, with
117 // A = [x2 - x1 y2 - y1 z2 - z1], x = [x - x1], b = [b21]
118 // [x3 - x1 y3 - y1 z3 - z1] [y - y1] [b31]
119 // [... ... ... ] [z - z1] [...]
120 // [xn - x1 yn - y1 zn - z1] [bn1]
121
122 // Hence, the linear system of equations solves (x - x1, y - y1, z - z1), we need a final step to
123 // add the reference point (x1, y1, z1) in order to solve (x, y, z).
124
125 if (!isReady()) {
126 throw new NotReadyException();
127 }
128 if (isLocked()) {
129 throw new LockedException();
130 }
131
132 try {
133 locked = true;
134
135 if (listener != null) {
136 listener.onSolveStart(this);
137 }
138
139 final var numberOfPositions = positions.length;
140 final var numberOfPositionsMinus1 = numberOfPositions - 1;
141 final var dims = getNumberOfDimensions();
142
143 final var a = new Matrix(numberOfPositionsMinus1, dims);
144 for (int i = 1, i2 = 0; i < numberOfPositions; i++, i2++) {
145 for (var j = 0; j < dims; j++) {
146 a.setElementAt(i2, j, positions[i].getInhomogeneousCoordinate(j)
147 - positions[0].getInhomogeneousCoordinate(j));
148 }
149 }
150
151 // reference point is first position mPositions[0] with distance mDistances[0]
152 final var referenceDistance = distances[0];
153 final var sqrRefDistance = referenceDistance * referenceDistance;
154 final var b = new double[numberOfPositionsMinus1];
155 for (int i = 1, i2 = 0; i < numberOfPositions; i++, i2++) {
156 final var ri = distances[i];
157 final var sqrRi = ri * ri;
158
159 // find distance between ri and r0
160 final var sqrRi0 = positions[i].sqrDistanceTo(positions[0]);
161 b[i2] = 0.5 * (sqrRefDistance - sqrRi + sqrRi0);
162 }
163
164 estimatedPositionCoordinates = Utils.solve(a, b);
165
166 // add position of reference point
167 for (var i = 0; i < dims; i++) {
168 estimatedPositionCoordinates[i] += positions[0].getInhomogeneousCoordinate(i);
169 }
170
171 if (listener != null) {
172 listener.onSolveEnd(this);
173 }
174 } catch (final AlgebraException e) {
175 throw new LaterationException(e);
176 } finally {
177 locked = false;
178 }
179 }
180
181 /**
182 * Gets lateration solver type.
183 *
184 * @return lateration solver type.
185 */
186 @Override
187 public LaterationSolverType getType() {
188 return LaterationSolverType.INHOMOGENEOUS_LINEAR_TRILATERATION_SOLVER;
189 }
190 }