1 /*
2 * Copyright (C) 2019 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.SingularValueDecomposer;
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 homogeneous LMSE solution.
27 */
28 public abstract class HomogeneousLinearLeastSquaresLaterationSolver<P extends Point<?>> extends LaterationSolver<P> {
29
30 /**
31 * Constructor.
32 */
33 protected HomogeneousLinearLeastSquaresLaterationSolver() {
34 super();
35 }
36
37 /**
38 * Constructor.
39 *
40 * @param positions known positions of static nodes.
41 * @param distances euclidean distances from static nodes to mobile node.
42 * @throws IllegalArgumentException if either positions or distances are null,
43 * don't have the same length or their length is smaller than required points.
44 */
45 protected HomogeneousLinearLeastSquaresLaterationSolver(final P[] positions, final double[] distances) {
46 super(positions, distances);
47 }
48
49 /**
50 * Constructor.
51 *
52 * @param listener listener to be notified of events raised by this instance.
53 */
54 protected HomogeneousLinearLeastSquaresLaterationSolver(final LaterationSolverListener<P> listener) {
55 super(listener);
56 }
57
58 /**
59 * Constructor.
60 *
61 * @param positions known positions of static nodes.
62 * @param distances euclidean distances from static nodes to mobile node.
63 * @param listener listener to be notified of events raised by this instance.
64 * @throws IllegalArgumentException if either position or distances are null,
65 * don't have the same length or their length is smaller than required points.
66 */
67 protected HomogeneousLinearLeastSquaresLaterationSolver(
68 final P[] positions, final double[] distances, final LaterationSolverListener<P> listener) {
69 super(positions, distances, listener);
70 }
71
72 /**
73 * Solves the lateration problem.
74 *
75 * @throws LaterationException if lateration fails.
76 * @throws NotReadyException if solver is not ready.
77 * @throws LockedException if instance is busy solving the lateration problem.
78 */
79 @Override
80 public void solve() throws LaterationException, NotReadyException, LockedException {
81 // The implementation on this method follows the algorithm bellow.
82
83 // Having 3 2D circles:
84 // c1x, c1y, r1
85 // c2x, c2y, r2
86 // c3x, c3y, r3
87 // where (c1x, c1y) are the coordinates of 1st circle center and r1 is its radius.
88 // (c2x, c2y) are the coordinates of 2nd circle center and r2 is its radius.
89 // (c3x, c3y) are the coordinates of 3rd circle center and r3 is its radius.
90
91 // The equations of the circles are as follows:
92 // (x - c1x)^2 + (y - c1y)^2 = r1^2
93 // (x - c2x)^2 + (y - c2y)^2 = r2^2
94 // (x - c3x)^2 + (y - c3y)^2 = r3^2
95
96 // x^2 - 2*c1x*x + c1x^2 + y^2 - 2*c1y*y + c1y^2 = r1^2
97 // x^2 - 2*c2x*x + c2x^2 + y^2 - 2*c2y*y + c2y^2 = r2^2
98 // x^2 - 2*c3x*x + c3x^2 + y^2 - 2*c3y*y + c3y^2 = r3^2
99
100 // remove 1st equation from others (we use 1st point as reference)
101
102 // x^2 - 2*c2x*x + c2x^2 + y^2 - 2*c2y*y + c2y^2 - (x^2 - 2*c1x*x + c1x^2 + y^2 - 2*c1y*y + c1y^2) = r2^2 - r1^2
103 // x^2 - 2*c3x*x + c3x^2 + y^2 - 2*c3y*y + c3y^2 - (x^2 - 2*c1x*x + c1x^2 + y^2 - 2*c1y*y + c1y^2) = r3^2 - r1^2
104
105 // - 2*c2x*x + c2x^2 - 2*c2y*y + c2y^2 + 2*c1x*x - c1x^2 + 2*c1y*y - c1y^2 = r2^2 - r1^2
106 // - 2*c3x*x + c3x^2 - 2*c3y*y + c3y^2 + 2*c1x*x - c1x^2 + 2*c1y*y - c1y^2 = r3^2 - r1^2
107
108 // 2*(c1x - c2x)*x + c2x^2 + 2*(c1y - c2y)*y + c2y^2 - c1x^2 - c1y^2 = r2^2 - r1^2
109 // 2*(c1x - c3x)*x + c3x^2 + 2*(c1y - c3y)*y + c3y^2 - c1x^2 - c1y^2 = r3^2 - r1^2
110
111 // 2*(c1x - c2x)*x + 2*(c1y - c2y)*y = r2^2 - r1^2 + c1x^2 + c1y^2 - c2x^2 - c2y^2
112 // 2*(c1x - c3x)*x + 2*(c1y - c3y)*y = r3^2 - r1^2 + c1x^2 + c1y^2 - c3x^2 - c3y^2
113
114 // x and y are the inhomogeneous coordinates of the point (x,y) we want to find, we
115 // substitute such point by the corresponding homogeneous coordinates (x,y) = (x'/w', y'/w')
116
117 // Hence
118 // 2*(c1x - c2x)*x'/w' + 2*(c1y - c2y)*y'/w' = r2^2 - r1^2 + c1x^2 + c1y^2 - c2x^2 - c2y^2
119 // 2*(c1x - c3x)*x'/w' + 2*(c1y - c3y)*y'/w' = r3^2 - r1^2 + c1x^2 + c1y^2 - c3x^2 - c3y^2
120
121 // Multiplying by w' at both sides...
122 // 2*(c1x - c2x)*x' + 2*(c1y - c2y)*y' = (r2^2 - r1^2 + c1x^2 + c1y^2 - c2x^2 - c2y^2)*w'
123 // 2*(c1x - c3x)*x' + 2*(c1y - c3y)*y' = (r3^2 - r1^2 + c1x^2 + c1y^2 - c3x^2 - c3y^2)*w'
124
125 // Obtaining the following homogeneous equations
126 // 2*(c1x - c2x)*x' + 2*(c1y - c2y)*y' - (r2^2 - r1^2 + c1x^2 + c1y^2 - c2x^2 - c2y^2)*w' = 0
127 // 2*(c1x - c3x)*x' + 2*(c1y - c3y)*y' - (r3^2 - r1^2 + c1x^2 + c1y^2 - c3x^2 - c3y^2)*w' = 0
128
129 // Fixing signs...
130 // 2*(c1x - c2x)*x' + 2*(c1y - c2y)*y' + (r1^2 - r2^2 + c2x^2 + c2y^2 - c1x^2 - c1y^2)*w' = 0
131 // 2*(c1x - c3x)*x' + 2*(c1y - c3y)*y' + (r1^2 - r3^2 + c3x^2 + c3y^2 - c1x^2 - c1y^2)*w' = 0
132
133
134 // The homogeneous equations can be expressed as a linear system of homogeneous equations A*x = 0
135 // where the unknowns to be solved are (x', y', w') up to scale.
136
137 // [2*(c1x - c2x) 2*(c1y - c2y) r1^2 - r2^2 + c2x^2 + c2y^2 - c1x^2 - c1y^2][x'] = 0
138 // [2*(c1x - c3x) 2*(c1y - c3y) r1^2 - r3^2 + c3x^2 + c3y^2 - c1x^2 - c1y^2][y'] = 0
139 // [w']
140
141 // This can be solved by using the SVD decomposition of matrix A and picking the last column of
142 // resulting V matrix. At least 2 equations are required to find a solution, since 1 additional
143 // point is used as a reference, at least 3 points are required.
144
145 // For spheres the solution is analogous
146
147 // Having 4 3D spheres:
148 // c1x, c1y, c1z, r1
149 // c2x, c2y, c2z, r2
150 // c3x, c3y, c3z, r3
151 // c4x, c4y, c4z, r4
152 // where (c1x, c1y, c1z) are the coordinates of 1st sphere center and r1 is its radius.
153 // (c2x, c2y, c2z) are the coordinates of 2nd sphere center and r2 is its radius.
154 // (c3x, c3y, c3z) are the coordinates of 3rd sphere center and r3 is its radius.
155 // (c4x, c4y, c4z) are the coordinates of 4th sphere center and r4 is its radius.
156
157 // The equations of the spheres are as follows:
158 // (x - c1x)^2 + (y - c1y)^2 + (z - c1z)^2 = r1^2
159 // (x - c2x)^2 + (y - c2y)^2 + (z - c2z)^2 = r2^2
160 // (x - c3x)^2 + (y - c3y)^2 + (z - c3z)^2 = r3^2
161 // (x - c4x)^2 + (y - c4y)^2 + (z - c4z)^2 = r4^2
162
163 // x^2 - 2*c1x*x + c1x^2 + y^2 - 2*c1y*y + c1y^2 + z^2 - 2*c1z*z + c1z^2 = r1^2
164 // x^2 - 2*c2x*x + c2x^2 + y^2 - 2*c2y*y + c2y^2 + z^2 - 2*c2z*z + c2z^2 = r2^2
165 // x^2 - 2*c3x*x + c3x^2 + y^2 - 2*c3y*y + c3y^3 + z^2 - 2*c3z*z + c3z^2 = r3^2
166 // x^2 - 2*c4x*x + c4x^2 + y^2 - 2*c4y*y + c4y^2 + z^2 - 2*c4z*z + c4z^2 = r4^2
167
168 // remove 1st equation from others (we use 1st point as reference)
169 // x^2 - 2*c2x*x + c2x^2 + y^2 - 2*c2y*y + c2y^2 + z^2 - 2*c2z*z + c2z^2 - (x^2 - 2*c1x*x + c1x^2 + y^2 - 2*c1y*y + c1y^2 + z^2 - 2*c1z*z + c1z^2) = r2^2 - r1^2
170 // x^2 - 2*c3x*x + c3x^2 + y^2 - 2*c3y*y + c3y^3 + z^2 - 2*c3z*z + c3z^2 - (x^2 - 2*c1x*x + c1x^2 + y^2 - 2*c1y*y + c1y^2 + z^2 - 2*c1z*z + c1z^2) = r3^2 - r1^2
171 // x^2 - 2*c4x*x + c4x^2 + y^2 - 2*c4y*y + c4y^2 + z^2 - 2*c4z*z + c4z^2 - (x^2 - 2*c1x*x + c1x^2 + y^2 - 2*c1y*y + c1y^2 + z^2 - 2*c1z*z + c1z^2) = r4^2 - r1^2
172
173 // - 2*c2x*x + c2x^2 - 2*c2y*y + c2y^2 - 2*c2z*z + c2z^2 + 2*c1x*x - c1x^2 + 2*c1y*y - c1y^2 + 2*c1z*z - c1z^2 = r2^2 - r1^2
174 // - 2*c3x*x + c3x^2 - 2*c3y*y + c3y^3 - 2*c3z*z + c3z^2 + 2*c1x*x - c1x^2 + 2*c1y*y - c1y^2 + 2*c1z*z - c1z^2 = r3^2 - r1^2
175 // - 2*c4x*x + c4x^2 - 2*c4y*y + c4y^2 - 2*c4z*z + c4z^2 + 2*c1x*x - c1x^2 + 2*c1y*y - c1y^2 + 2*c1z*z - c1z^2 = r4^2 - r1^2
176
177 // 2*(c1x - c2x)*x + c2x^2 + 2*(c1y - c2y)*y + c2y^2 + 2*(c1z - c2z)*z + c2z^2 - c1x^2 - c1y^2 - c1z^2 = r2^2 - r1^2
178 // 2*(c1x - c3x)*x + c3x^2 + 2*(c1y - c3y)*y + c3y^3 + 2*(c1z - c3z)*z + c3z^2 - c1x^2 - c1y^2 - c1z^2 = r3^2 - r1^2
179 // 2*(c1x - c4x)*x + c4x^2 + 2*(c1y - c4y)*y + c4y^2 + 2*(c1z - c4z)*z + c4z^2 - c1x^2 - c1y^2 - c1z^2 = r4^2 - r1^2
180
181 // 2*(c1x - c2x)*x + 2*(c1y - c2y)*y + 2*(c1z - c2z)*z = r2^2 - r1^2 + c1x^2 + c1y^2 + c1z^2 - c2x^2 - c2y^2 - c2z^2
182 // 2*(c1x - c3x)*x + 2*(c1y - c3y)*y + 2*(c1z - c3z)*z = r3^2 - r1^2 + c1x^2 + c1y^2 + c1z^2 - c3x^2 - c3y^3 - c3z^2
183 // 2*(c1x - c4x)*x + 2*(c1y - c4y)*y + 2*(c1z - c4z)*z = r4^2 - r1^2 + c1x^2 + c1y^2 + c1z^2 - c4x^2 - c4y^2 - c4z^2
184
185 // x, y and z the inhomogeneous coordinates of the point (x,y,z) we want to find,
186 // we substitute such point by the corresponding homogeneous coordinates
187 // (x, y, z) = (x'/w', y'/w', z'/w')
188
189 // Hence
190 // 2*(c1x - c2x)*x'/w' + 2*(c1y - c2y)*y'/w' + 2*(c1z - c2z)*z'/w' = r2^2 - r1^2 + c1x^2 + c1y^2 + c1z^2 - c2x^2 - c2y^2 - c2z^2
191 // 2*(c1x - c3x)*x'/w' + 2*(c1y - c3y)*y'/w' + 2*(c1z - c3z)*z'/w' = r3^2 - r1^2 + c1x^2 + c1y^2 + c1z^2 - c3x^2 - c3y^3 - c3z^2
192 // 2*(c1x - c4x)*x'/w' + 2*(c1y - c4y)*y'/w' + 2*(c1z - c4z)*z'/w' = r4^2 - r1^2 + c1x^2 + c1y^2 + c1z^2 - c4x^2 - c4y^2 - c4z^2
193
194 // Multiplying by w' at both sides...
195 // 2*(c1x - c2x)*x' + 2*(c1y - c2y)*y' + 2*(c1z - c2z)*z' = (r2^2 - r1^2 + c1x^2 + c1y^2 + c1z^2 - c2x^2 - c2y^2 - c2z^2)*w'
196 // 2*(c1x - c3x)*x' + 2*(c1y - c3y)*y' + 2*(c1z - c3z)*z' = (r3^2 - r1^2 + c1x^2 + c1y^2 + c1z^2 - c3x^2 - c3y^3 - c3z^2)*w'
197 // 2*(c1x - c4x)*x' + 2*(c1y - c4y)*y' + 2*(c1z - c4z)*z' = (r4^2 - r1^2 + c1x^2 + c1y^2 + c1z^2 - c4x^2 - c4y^2 - c4z^2)*w'
198
199 // Obtaining the following homogeneous equations
200 // 2*(c1x - c2x)*x' + 2*(c1y - c2y)*y' + 2*(c1z - c2z)*z' - (r2^2 - r1^2 + c1x^2 + c1y^2 + c1z^2 - c2x^2 - c2y^2 - c2z^2)*w' = 0
201 // 2*(c1x - c3x)*x' + 2*(c1y - c3y)*y' + 2*(c1z - c3z)*z' - (r3^2 - r1^2 + c1x^2 + c1y^2 + c1z^2 - c3x^2 - c3y^3 - c3z^2)*w' = 0
202 // 2*(c1x - c4x)*x' + 2*(c1y - c4y)*y' + 2*(c1z - c4z)*z' - (r4^2 - r1^2 + c1x^2 + c1y^2 + c1z^2 - c4x^2 - c4y^2 - c4z^2)*w' = 0
203
204 // Fixing signs...
205 // 2*(c1x - c2x)*x' + 2*(c1y - c2y)*y' + 2*(c1z - c2z)*z' + (r1^2 - r2^2 + c2x^2 + c2y^2 + c2z^2 - c1x^2 - c1y^2 - c1z^2)*w' = 0
206 // 2*(c1x - c3x)*x' + 2*(c1y - c3y)*y' + 2*(c1z - c3z)*z' + (r1^2 - r3^2 + c3x^2 + c3y^3 + c3z^2 - c1x^2 - c1y^2 - c1z^2)*w' = 0
207 // 2*(c1x - c4x)*x' + 2*(c1y - c4y)*y' + 2*(c1z - c4z)*z' + (r1^2 - r4^2 + c4x^2 + c4y^2 + c4z^2 - c1x^2 - c1y^2 - c1z^2)*w' = 0
208
209 // The homogeneous equations can be expressed as a linear system of homogeneous equations
210 // where the unknowns to be solved are (x', y', z', w') up to scale.
211
212 // [2*(c1x - c2x) 2*(c1y - c2y) 2*(c1z - c2z) r1^2 - r2^2 + c2x^2 + c2y^2 + c2z^2 - c1x^2 - c1y^2 - c1z^2][x'] = 0
213 // [2*(c1x - c3x) 2*(c1y - c3y) 2*(c1z - c3z) r1^2 - r3^2 + c3x^2 + c3y^3 + c3z^2 - c1x^2 - c1y^2 - c1z^2][y'] = 0
214 // [2*(c1x - c4x) 2*(c1y - c4y) 2*(c1z - c4z) r1^2 - r4^2 + c4x^2 + c4y^2 + c4z^2 - c1x^2 - c1y^2 - c1z^2][z'] = 0
215 // [w']
216
217 // This can be solved by using the SVD decomposition of matrix A and picking the last column of
218 // resulting V matrix. At least 3 equations are required to find a solution, since 1 additional
219 // point is used as a reference, at least 4 points are required.
220
221 if (!isReady()) {
222 throw new NotReadyException();
223 }
224 if (isLocked()) {
225 throw new LockedException();
226 }
227
228 try {
229 locked = true;
230
231 if (listener != null) {
232 listener.onSolveStart(this);
233 }
234
235 final var numberOfPositions = positions.length;
236 final var numberOfPositionsMinus1 = numberOfPositions - 1;
237 final var dims = getNumberOfDimensions();
238
239 final var referenceDistance = distances[0];
240 final var sqrRefDistance = referenceDistance * referenceDistance;
241 final var refPosition = positions[0];
242
243 var sqrRefNorm = 0.0;
244 for (var j = 0; j < dims; j++) {
245 final var coord = refPosition.getInhomogeneousCoordinate(j);
246 sqrRefNorm += coord * coord;
247 }
248
249 final var a = new Matrix(numberOfPositionsMinus1, dims + 1);
250 for (int i = 1, i2 = 0; i < numberOfPositions; i++, i2++) {
251
252 var sqrNorm = 0.0;
253 for (var j = 0; j < dims; j++) {
254 final var refCoord = refPosition.getInhomogeneousCoordinate(j);
255 final var coord = positions[i].getInhomogeneousCoordinate(j);
256
257 a.setElementAt(i2, j, 2.0 * (refCoord - coord));
258
259 sqrNorm += coord * coord;
260 }
261
262 final var distance = distances[i];
263 final var sqrDistance = distance * distance;
264 a.setElementAt(i2, dims, sqrRefDistance - sqrDistance + sqrNorm - sqrRefNorm);
265 }
266
267 final var decomposer = new SingularValueDecomposer(a);
268 decomposer.decompose();
269
270 final var nullity = decomposer.getNullity();
271 if (nullity > 1) {
272 // linear system of equations is degenerate (does not have enough rank),
273 // probably because there are dependencies or repeated data between
274 // points
275 throw new LaterationException();
276 }
277
278 final var v = decomposer.getV();
279 final var homogeneousEstimatedPositionCoordinates = v.getSubmatrixAsArray(
280 0, dims, dims, dims);
281
282 final var w = homogeneousEstimatedPositionCoordinates[dims];
283 estimatedPositionCoordinates = new double[dims];
284 for (var j = 0; j < dims; j++) {
285 estimatedPositionCoordinates[j] = homogeneousEstimatedPositionCoordinates[j] / w;
286 }
287
288 if (listener != null) {
289 listener.onSolveEnd(this);
290 }
291 } catch (final AlgebraException e) {
292 throw new LaterationException(e);
293 } finally {
294 locked = false;
295 }
296 }
297
298 /**
299 * Gets lateration solver type.
300 *
301 * @return lateration solver type.
302 */
303 @Override
304 public LaterationSolverType getType() {
305 return LaterationSolverType.HOMOGENEOUS_LINEAR_TRILATERATION_SOLVER;
306 }
307 }