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.geometry.Point;
21 import com.irurueta.navigation.LockedException;
22 import com.irurueta.navigation.NotReadyException;
23 import com.irurueta.numerical.NumericalException;
24 import com.irurueta.numerical.fitting.FittingException;
25 import com.irurueta.numerical.fitting.LevenbergMarquardtMultiDimensionFitter;
26 import com.irurueta.numerical.fitting.LevenbergMarquardtMultiDimensionFunctionEvaluator;
27
28 import java.util.Arrays;
29
30 /**
31 * Solves a Trilateration problem with an instance of the least squares optimizer.
32 * By solving the lateration problem linearly, this class is able to estimate
33 * the covariance of estimated position.
34 * To achieve better results, it is usually better to provide an initial coarse
35 * solution.
36 * This class is base on the implementation found at:
37 * <a href="https://github.com/lemmingapex/trilateration">https://github.com/lemmingapex/trilateration</a>
38 *
39 * @param <P> a {@link Point} type.
40 */
41 public abstract class NonLinearLeastSquaresLaterationSolver<P extends Point<?>> extends LaterationSolver<P> {
42
43 /**
44 * Default standard deviation assumed for provided distances when none is
45 * explicitly provided.
46 */
47 public static final double DEFAULT_DISTANCE_STANDARD_DEVIATION = 1.0e-3;
48
49 /**
50 * Levenberg-Marquardt fitter to find a non-linear solution.
51 */
52 private final LevenbergMarquardtMultiDimensionFitter fitter = new LevenbergMarquardtMultiDimensionFitter();
53
54 /**
55 * Estimated covariance matrix for estimated position.
56 */
57 private Matrix covariance;
58
59 /**
60 * Estimated chi square value.
61 */
62 private double chiSq;
63
64 /**
65 * Initial position to start lateration solving.
66 * If not defined, centroid of provided position points will be used.
67 */
68 private P initialPosition;
69
70 /**
71 * Standard deviations of provided distances.
72 */
73 private double[] distanceStandardDeviations;
74
75 /**
76 * Constructor.
77 */
78 protected NonLinearLeastSquaresLaterationSolver() {
79 super();
80 }
81
82 /**
83 * Constructor.
84 *
85 * @param positions known positions of static nodes.
86 * @param distances euclidean distances from static nodes to mobile node.
87 * @throws IllegalArgumentException if either positions or distances are null, don't have the same length or their
88 * length is smaller than required 2 points.
89 */
90 protected NonLinearLeastSquaresLaterationSolver(final P[] positions, final double[] distances) {
91 super();
92 internalSetPositionsAndDistances(positions, distances);
93 }
94
95 /**
96 * Constructor.
97 *
98 * @param initialPosition initial position to start lateration solving.
99 */
100 protected NonLinearLeastSquaresLaterationSolver(final P initialPosition) {
101 super();
102 this.initialPosition = initialPosition;
103 }
104
105 /**
106 * Constructor.
107 *
108 * @param positions known positions of static nodes.
109 * @param distances euclidean distances from static nodes to mobile node.
110 * @param initialPosition initial position to start lateration solving.
111 * @throws IllegalArgumentException if either positions or distances are null, don't have the same length or their
112 * length is smaller than required (3 for 2D points or 4 for 3D points) or fitter
113 * is null.
114 */
115 protected NonLinearLeastSquaresLaterationSolver(
116 final P[] positions, final double[] distances, final P initialPosition) {
117 this(initialPosition);
118 internalSetPositionsAndDistances(positions, distances);
119 }
120
121 /**
122 * Constructor.
123 *
124 * @param listener listener to be notified of events raised by this instance.
125 */
126 protected NonLinearLeastSquaresLaterationSolver(final LaterationSolverListener<P> listener) {
127 super(listener);
128 }
129
130 /**
131 * Constructor.
132 *
133 * @param positions known positions of static nodes.
134 * @param distances euclidean distances from static nodes to mobile node.
135 * @param listener listener to be notified of events raised by this instance.
136 * @throws IllegalArgumentException if either positions or distances are null, don't have the same length or their
137 * length is smaller than required 2 points.
138 */
139 protected NonLinearLeastSquaresLaterationSolver(
140 final P[] positions, final double[] distances, final LaterationSolverListener<P> listener) {
141 super(listener);
142 internalSetPositionsAndDistances(positions, distances);
143 }
144
145 /**
146 * Constructor.
147 *
148 * @param initialPosition initial position to start lateration solving.
149 * @param listener listener to be notified of events raised by this instance.
150 */
151 protected NonLinearLeastSquaresLaterationSolver(
152 final P initialPosition, final LaterationSolverListener<P> listener) {
153 super(listener);
154 this.initialPosition = initialPosition;
155 }
156
157 /**
158 * Constructor.
159 *
160 * @param positions known positions of static nodes.
161 * @param distances euclidean distances from static nodes to mobile node.
162 * @param initialPosition initial position to start lateration solving.
163 * @param listener listener to be notified of events raised by this instance.
164 * @throws IllegalArgumentException if either positions or distances are null, don't have the same length or their
165 * length is smaller than required (3 for 2D points or 4 for 3D points) or fitter
166 * is null.
167 */
168 protected NonLinearLeastSquaresLaterationSolver(
169 final P[] positions, final double[] distances, final P initialPosition,
170 final LaterationSolverListener<P> listener) {
171 this(initialPosition, listener);
172 internalSetPositionsAndDistances(positions, distances);
173 }
174
175 /**
176 * Constructor.
177 *
178 * @param positions known positions of static nodes.
179 * @param distances euclidean distances from static nodes to mobile node.
180 * @param distanceStandardDeviations standard deviations of provided measured distances.
181 * @throws IllegalArgumentException if either positions, distances or standard deviations
182 * are null, don't have the same length of their length is smaller than required
183 * (2 points).
184 */
185 protected NonLinearLeastSquaresLaterationSolver(
186 final P[] positions, final double[] distances, final double[] distanceStandardDeviations) {
187 super();
188 internalSetPositionsDistancesAndStandardDeviations(positions, distances, distanceStandardDeviations);
189 }
190
191 /**
192 * Constructor.
193 *
194 * @param positions known positions of static nodes.
195 * @param distances euclidean distances from static nodes to mobile node.
196 * @param distanceStandardDeviations standard deviations of provided measured distances.
197 * @param initialPosition initial position to start lateration solving.
198 * @throws IllegalArgumentException if either positions, distances or standard deviations
199 * are null, don't have the same length of their length is smaller than required
200 * (2 points).
201 */
202 protected NonLinearLeastSquaresLaterationSolver(
203 final P[] positions, final double[] distances, final double[] distanceStandardDeviations,
204 final P initialPosition) {
205 this(initialPosition);
206 internalSetPositionsDistancesAndStandardDeviations(positions, distances, distanceStandardDeviations);
207 }
208
209 /**
210 * Constructor.
211 *
212 * @param positions known positions of static nodes.
213 * @param distances euclidean distances from static nodes to mobile node.
214 * @param distanceStandardDeviations standard deviations of provided measured distances.
215 * @param listener listener to be notified of events raised by this instance.
216 * @throws IllegalArgumentException if either positions, distances or standard deviations
217 * are null, don't have the same length of their length is smaller than required
218 * (2 points).
219 */
220 protected NonLinearLeastSquaresLaterationSolver(
221 final P[] positions, final double[] distances, final double[] distanceStandardDeviations,
222 final LaterationSolverListener<P> listener) {
223 super(listener);
224 internalSetPositionsDistancesAndStandardDeviations(positions, distances, distanceStandardDeviations);
225 }
226
227 /**
228 * Constructor.
229 *
230 * @param positions known positions of static nodes.
231 * @param distances euclidean distances from static nodes to mobile node.
232 * @param distanceStandardDeviations standard deviations of provided measured distances.
233 * @param initialPosition initial position to start lateration solving.
234 * @param listener listener to be notified of events raised by this instance.
235 * @throws IllegalArgumentException if either positions, distances or standard deviations
236 * are null, don't have the same length of their length is smaller than required
237 * (2 points).
238 */
239 protected NonLinearLeastSquaresLaterationSolver(
240 final P[] positions, final double[] distances, final double[] distanceStandardDeviations,
241 final P initialPosition, final LaterationSolverListener<P> listener) {
242 this(initialPosition, listener);
243 internalSetPositionsDistancesAndStandardDeviations(positions, distances, distanceStandardDeviations);
244 }
245
246 /**
247 * Sets known positions and Euclidean distances.
248 * If any distance value is zero or negative, it will be fixed assuming an EPSILON value.
249 *
250 * @param positions known positions of static nodes.
251 * @param distances euclidean distances from static nodes to mobile node.
252 * @throws IllegalArgumentException if either positions or distances are null, don't have the same length or their
253 * length is smaller than required (2 points).
254 * @throws LockedException if instance is busy solving the lateration problem.
255 */
256 @Override
257 public void setPositionsAndDistances(final P[] positions, final double[] distances) throws LockedException {
258 if (isLocked()) {
259 throw new LockedException();
260 }
261 internalSetPositionsAndDistances(positions, distances);
262 }
263
264 /**
265 * Sets known positions, Euclidean distances and the respective standard deviations of
266 * measured distances.
267 * If any distance value is zero or negative, it will be fixed assuming an EPSILON value.
268 *
269 * @param positions known positions of static nodes.
270 * @param distances euclidean distances from static nodes to mobile node.
271 * @param distanceStandardDeviations standard deviations of provided measured distances.
272 * @throws IllegalArgumentException if either positions, distances or standard deviations
273 * are null, don't have the same length of their length is smaller than required
274 * (2 points).
275 * @throws LockedException if instance is busy solving the trilateration problem.
276 */
277 public void setPositionsDistancesAndStandardDeviations(
278 final P[] positions, final double[] distances, final double[] distanceStandardDeviations)
279 throws LockedException {
280 if (isLocked()) {
281 throw new LockedException();
282 }
283 internalSetPositionsDistancesAndStandardDeviations(positions, distances, distanceStandardDeviations);
284 }
285
286 /**
287 * Gets standard deviations of provided distances.
288 *
289 * @return standard deviations of provided distances.
290 */
291 public double[] getDistanceStandardDeviations() {
292 return distanceStandardDeviations;
293 }
294
295 /**
296 * Gets estimated covariance matrix for estimated position.
297 *
298 * @return estimated covariance matrix for estimated position.
299 */
300 public Matrix getCovariance() {
301 return covariance;
302 }
303
304 /**
305 * Gets estimated chi square value.
306 *
307 * @return estimated chi square value.
308 */
309 public double getChiSq() {
310 return chiSq;
311 }
312
313 /**
314 * Gets initial position to start lateration solving.
315 * If not defined, centroid of provided position points will be used to start lateration.
316 *
317 * @return initial position to start lateration.
318 */
319 public P getInitialPosition() {
320 return initialPosition;
321 }
322
323 /**
324 * Sets initial position to start lateration solving.
325 * If not defined, centroid of provided position points will be used to start lateration.
326 *
327 * @param initialPosition initial position to start lateration.
328 * @throws LockedException if instance is busy solving the lateration problem.
329 */
330 public void setInitialPosition(final P initialPosition) throws LockedException {
331 if (isLocked()) {
332 throw new LockedException();
333 }
334 this.initialPosition = initialPosition;
335 }
336
337 /**
338 * Solves the lateration problem.
339 *
340 * @throws LaterationException if lateration fails.
341 * @throws NotReadyException is solver is not ready.
342 * @throws LockedException if instance is busy solving the lateration problem.
343 */
344 @Override
345 public void solve() throws LaterationException, NotReadyException, LockedException {
346 if (!isReady()) {
347 throw new NotReadyException();
348 }
349 if (isLocked()) {
350 throw new LockedException();
351 }
352
353 try {
354 locked = true;
355
356 if (listener != null) {
357 listener.onSolveStart(this);
358 }
359
360 setupFitter();
361
362 fitter.fit();
363
364 // estimated position
365 estimatedPositionCoordinates = fitter.getA();
366 covariance = fitter.getCovar();
367 chiSq = fitter.getChisq();
368
369 if (listener != null) {
370 listener.onSolveEnd(this);
371 }
372 } catch (final NumericalException e) {
373 throw new LaterationException(e);
374 } finally {
375 locked = false;
376 }
377 }
378
379 /**
380 * Gets lateration solver type.
381 *
382 * @return lateration solver type.
383 */
384 @Override
385 public LaterationSolverType getType() {
386 return LaterationSolverType.NON_LINEAR_TRILATERATION_SOLVER;
387 }
388
389 /**
390 * Internally sets known positions and Euclidean distances.
391 * If any distance value is zero or negative, it will be fixed assuming an EPSILON value.
392 *
393 * @param positions known positions of static nodes.
394 * @param distances euclidean distances from static nodes to mobile node.
395 * @throws IllegalArgumentException if either positions or distances are null, don't have the same length or their
396 * length is smaller than required (2 points).
397 */
398 @Override
399 protected void internalSetPositionsAndDistances(final P[] positions, final double[] distances) {
400 super.internalSetPositionsAndDistances(positions, distances);
401
402 // initialize distances standard deviations to default values
403 distanceStandardDeviations = new double[distances.length];
404 Arrays.fill(distanceStandardDeviations, DEFAULT_DISTANCE_STANDARD_DEVIATION);
405 }
406
407 /**
408 * Internally sets known positions, Euclidean distances and the respective standard deviations of
409 * measured distances.
410 * If any distance value is zero or negative, it will be fixed assuming an EPSILON value.
411 *
412 * @param positions known positions of static nodes.
413 * @param distances euclidean distances from static nodes to mobile node.
414 * @param distanceStandardDeviations standard deviations of provided measured distances.
415 * @throws IllegalArgumentException if either positions, distances or standard deviations
416 * are null, don't have the same length of their length is smaller than required
417 * (2 points).
418 */
419 protected void internalSetPositionsDistancesAndStandardDeviations(
420 final P[] positions, final double[] distances, final double[] distanceStandardDeviations) {
421 if (distanceStandardDeviations == null || distances == null) {
422 throw new IllegalArgumentException();
423 }
424 if (distances.length != distanceStandardDeviations.length) {
425 throw new IllegalArgumentException();
426 }
427
428 super.internalSetPositionsAndDistances(positions, distances);
429 this.distanceStandardDeviations = distanceStandardDeviations;
430 }
431
432 /**
433 * Setups fitter to solve lateration problem.
434 *
435 * @throws FittingException if Levenberg-Marquardt fitting fails.
436 */
437 private void setupFitter() throws FittingException {
438 fitter.setFunctionEvaluator(new LevenbergMarquardtMultiDimensionFunctionEvaluator() {
439 @Override
440 public int getNumberOfDimensions() {
441 return NonLinearLeastSquaresLaterationSolver.this.getNumberOfDimensions();
442 }
443
444 @Override
445 public double[] createInitialParametersArray() {
446 final var dims = getNumberOfDimensions();
447 final var initial = new double[dims];
448
449 if (initialPosition == null) {
450 // use centroid of positions as initial value
451 final var numSamples = positions.length;
452
453 for (var i = 0; i < dims; i++) {
454 initial[i] = 0.0;
455 for (final var position : positions) {
456 initial[i] += position.getInhomogeneousCoordinate(i) / numSamples;
457 }
458 }
459 } else {
460 // use provided initial position
461 for (var i = 0; i < dims; i++) {
462 initial[i] = initialPosition.getInhomogeneousCoordinate(i);
463 }
464 }
465
466 return initial;
467 }
468
469 @Override
470 public double evaluate(final int i, final double[] point, final double[] params,
471 final double[] derivatives) {
472 // we want to estimate the position contained as inhomogeneous coordinates in params array.
473 // the function evaluates the distance to provided point respect to current parameter
474 // (estimated position)
475 // sqrDist = (x - px)^2 + (y - py)^2
476 // grad = [2*(x - px), 2*(y - py)]
477 final var dims = getNumberOfDimensions();
478 var result = 0.0;
479 for (var j = 0; j < dims; j++) {
480 final var param = params[j];
481 final var diff = param - point[j];
482 result += diff * diff;
483 derivatives[j] = 2.0 * diff;
484 }
485
486 return result;
487 }
488 });
489
490 final var dims = getNumberOfDimensions();
491 try {
492 final var x = new Matrix(positions.length, dims);
493 final var y = new double[positions.length];
494 for (var i = 0; i < positions.length; i++) {
495 for (var j = 0; j < dims; j++) {
496 x.setElementAt(i, j, positions[i].getInhomogeneousCoordinate(j));
497 }
498
499 final var dist = distances[i];
500 y[i] = dist * dist;
501 }
502
503 fitter.setInputData(x, y, distanceStandardDeviations);
504 } catch (final AlgebraException ignore) {
505 // never happens
506 }
507 }
508 }