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.indoor.position;
17
18 import com.irurueta.geometry.Point3D;
19 import com.irurueta.navigation.LockedException;
20 import com.irurueta.navigation.indoor.RadioSource;
21 import com.irurueta.navigation.indoor.RadioSourceLocated;
22 import com.irurueta.navigation.indoor.RangingFingerprint;
23 import com.irurueta.navigation.indoor.RangingReading;
24 import com.irurueta.navigation.lateration.PROMedSRobustLateration3DSolver;
25 import com.irurueta.numerical.robust.RobustEstimatorMethod;
26
27 import java.util.List;
28
29 /**
30 * Robustly estimates 3D position using located radio sources and their
31 * ranging readings at unknown locations and using PROMedS algorithm to discard outliers.
32 * This kind of estimator can be used to robustly determine the 3D position of a given
33 * device by getting readings at an unknown location of different radio sources whose
34 * 3D locations are known.
35 */
36 public class PROMedSRobustRangingPositionEstimator3D extends RobustRangingPositionEstimator3D {
37
38 /**
39 * Quality scores corresponding to each provided located radio source.
40 * The larger the score value the better the quality of the radio source.
41 */
42 private double[] sourceQualityScores;
43
44 /**
45 * Quality scores corresponding to each reading within provided fingerprint.
46 * The larger the score value the better the quality of the reading.
47 */
48 private double[] fingerprintReadingsQualityScores;
49
50 /**
51 * Constructor.
52 */
53 public PROMedSRobustRangingPositionEstimator3D() {
54 super();
55 init();
56 }
57
58 /**
59 * Constructor.
60 *
61 * @param sources located radio sources used for lateration.
62 * @throws IllegalArgumentException if provided sources is null or the number of
63 * provided sources is less than the required minimum.
64 */
65 public PROMedSRobustRangingPositionEstimator3D(final List<? extends RadioSourceLocated<Point3D>> sources) {
66 super();
67 init();
68 internalSetSources(sources);
69 }
70
71 /**
72 * Constructor.
73 *
74 * @param fingerprint fingerprint containing ranging readings at an unknown location
75 * for provided located radio sources.
76 * @throws IllegalArgumentException if provided fingerprint is null.
77 */
78 public PROMedSRobustRangingPositionEstimator3D(
79 final RangingFingerprint<? extends RadioSource, ? extends RangingReading<?
80 extends RadioSource>> fingerprint) {
81 super();
82 init();
83 internalSetFingerprint(fingerprint);
84 }
85
86 /**
87 * Constructor.
88 *
89 * @param sources located radio sources used for lateration.
90 * @param fingerprint fingerprint containing ranging readings at an unknown location
91 * for provided located radio sources.
92 * @throws IllegalArgumentException if either provided sources or fingerprint is null
93 * or the number of provided sources is less than the required minimum.
94 */
95 public PROMedSRobustRangingPositionEstimator3D(
96 final List<? extends RadioSourceLocated<Point3D>> sources,
97 final RangingFingerprint<? extends RadioSource, ? extends RangingReading<?
98 extends RadioSource>> fingerprint) {
99 super();
100 init();
101 internalSetSources(sources);
102 internalSetFingerprint(fingerprint);
103 }
104
105 /**
106 * Constructor.
107 *
108 * @param listener listener in charge of handling events.
109 */
110 public PROMedSRobustRangingPositionEstimator3D(final RobustRangingPositionEstimatorListener<Point3D> listener) {
111 super(listener);
112 init();
113 }
114
115 /**
116 * Constructor.
117 *
118 * @param sources located radio sources used for lateration.
119 * @param listener listener in charge of handling events.
120 * @throws IllegalArgumentException if provided sources is null or the number of
121 * provided sources is less than the required minimum.
122 */
123 public PROMedSRobustRangingPositionEstimator3D(
124 final List<? extends RadioSourceLocated<Point3D>> sources,
125 final RobustRangingPositionEstimatorListener<Point3D> listener) {
126 super(listener);
127 init();
128 internalSetSources(sources);
129 }
130
131 /**
132 * Constructor.
133 *
134 * @param fingerprint fingerprint containing ranging readings at an unknown location
135 * for provided location radio sources.
136 * @param listener listener in charge of handling events.
137 * @throws IllegalArgumentException if provided fingerprint is null.
138 */
139 public PROMedSRobustRangingPositionEstimator3D(
140 final RangingFingerprint<? extends RadioSource, ? extends RangingReading<?
141 extends RadioSource>> fingerprint,
142 final RobustRangingPositionEstimatorListener<Point3D> listener) {
143 super(listener);
144 init();
145 internalSetFingerprint(fingerprint);
146 }
147
148 /**
149 * Constructor.
150 *
151 * @param sources located radio sources used for lateration.
152 * @param fingerprint fingerprint containing ranging readings at an unknown location
153 * for provided located radio sources.
154 * @param listener listener in charge of handling events.
155 * @throws IllegalArgumentException if either provided sources or fingerprint is
156 * null or the number of provided sources is less than the required minimum.
157 */
158 public PROMedSRobustRangingPositionEstimator3D(
159 final List<? extends RadioSourceLocated<Point3D>> sources,
160 final RangingFingerprint<? extends RadioSource, ? extends RangingReading<?
161 extends RadioSource>> fingerprint,
162 final RobustRangingPositionEstimatorListener<Point3D> listener) {
163 super(listener);
164 init();
165 internalSetSources(sources);
166 internalSetFingerprint(fingerprint);
167 }
168
169 /**
170 * Constructor.
171 *
172 * @param sourceQualityScores quality scores corresponding to
173 * each provided located radio source.
174 * The larger the score value the better
175 * the quality of the radio source.
176 * @param fingerprintReadingsQualityScores quality scores corresponding to readings
177 * within provided fingerprint. The larger
178 * the score the better the quality of the
179 * reading.
180 */
181 public PROMedSRobustRangingPositionEstimator3D(
182 final double[] sourceQualityScores, final double[] fingerprintReadingsQualityScores) {
183 this();
184 internalSetSourceQualityScores(sourceQualityScores);
185 internalSetFingerprintReadingsQualityScores(fingerprintReadingsQualityScores);
186 }
187
188 /**
189 * Constructor.
190 *
191 * @param sourceQualityScores quality scores corresponding to
192 * each provided located radio source.
193 * The larger the score value the better
194 * the quality of the radio source.
195 * @param fingerprintReadingsQualityScores quality scores corresponding to readings
196 * within provided fingerprint. The larger
197 * the score the better the quality of the
198 * reading.
199 * @param sources located radio sources used for
200 * lateration.
201 * @throws IllegalArgumentException if provided sources is null or the number of
202 * provided sources is less than the required minimum.
203 */
204 public PROMedSRobustRangingPositionEstimator3D(
205 final double[] sourceQualityScores, final double[] fingerprintReadingsQualityScores,
206 final List<? extends RadioSourceLocated<Point3D>> sources) {
207 this(sources);
208 internalSetSourceQualityScores(sourceQualityScores);
209 internalSetFingerprintReadingsQualityScores(fingerprintReadingsQualityScores);
210 }
211
212 /**
213 * Constructor.
214 *
215 * @param sourceQualityScores quality scores corresponding to
216 * each provided located radio source.
217 * The larger the score value the better
218 * the quality of the radio source.
219 * @param fingerprintReadingsQualityScores quality scores corresponding to readings
220 * within provided fingerprint. The larger
221 * the score the better the quality of the
222 * reading.
223 * @param fingerprint fingerprint containing ranging readings at
224 * an unknown location for provided located
225 * radio sources.
226 * @throws IllegalArgumentException if provided fingerprint is null.
227 */
228 public PROMedSRobustRangingPositionEstimator3D(
229 final double[] sourceQualityScores, final double[] fingerprintReadingsQualityScores,
230 final RangingFingerprint<? extends RadioSource, ? extends RangingReading<?
231 extends RadioSource>> fingerprint) {
232 this(fingerprint);
233 internalSetSourceQualityScores(sourceQualityScores);
234 internalSetFingerprintReadingsQualityScores(fingerprintReadingsQualityScores);
235 }
236
237 /**
238 * Constructor.
239 *
240 * @param sourceQualityScores quality scores corresponding to
241 * each provided located radio source.
242 * The larger the score value the better
243 * the quality of the radio source.
244 * @param fingerprintReadingsQualityScores quality scores corresponding to readings
245 * within provided fingerprint. The larger
246 * the score the better the quality of the
247 * reading.
248 * @param sources located radio sources used for
249 * lateration.
250 * @param fingerprint fingerprint containing ranging readings at
251 * an unknown location for provided located
252 * radio sources.
253 * @throws IllegalArgumentException if either provided sources or fingerprint is null
254 * or the number of provided sources is less than the required minimum.
255 */
256 public PROMedSRobustRangingPositionEstimator3D(
257 final double[] sourceQualityScores, final double[] fingerprintReadingsQualityScores,
258 final List<? extends RadioSourceLocated<Point3D>> sources,
259 final RangingFingerprint<? extends RadioSource, ? extends RangingReading<?
260 extends RadioSource>> fingerprint) {
261 this(sources, fingerprint);
262 internalSetSourceQualityScores(sourceQualityScores);
263 internalSetFingerprintReadingsQualityScores(fingerprintReadingsQualityScores);
264 }
265
266 /**
267 * Constructor.
268 *
269 * @param sourceQualityScores quality scores corresponding to
270 * each provided located radio source.
271 * The larger the score value the better
272 * the quality of the radio source.
273 * @param fingerprintReadingsQualityScores quality scores corresponding to readings
274 * within provided fingerprint. The larger
275 * the score the better the quality of the
276 * reading.
277 * @param listener listener in charge of handling events.
278 */
279 public PROMedSRobustRangingPositionEstimator3D(
280 final double[] sourceQualityScores, final double[] fingerprintReadingsQualityScores,
281 final RobustRangingPositionEstimatorListener<Point3D> listener) {
282 this(listener);
283 internalSetSourceQualityScores(sourceQualityScores);
284 internalSetFingerprintReadingsQualityScores(fingerprintReadingsQualityScores);
285 }
286
287 /**
288 * Constructor.
289 *
290 * @param sourceQualityScores quality scores corresponding to
291 * each provided located radio source.
292 * The larger the score value the better
293 * the quality of the radio source.
294 * @param fingerprintReadingsQualityScores quality scores corresponding to readings
295 * within provided fingerprint. The larger
296 * the score the better the quality of the
297 * reading.
298 * @param sources located radio sources used for
299 * lateration.
300 * @param listener listener in charge of handling events.
301 * @throws IllegalArgumentException if provided sources is null or the number of
302 * provided sources is less than the required minimum.
303 */
304 public PROMedSRobustRangingPositionEstimator3D(
305 final double[] sourceQualityScores, final double[] fingerprintReadingsQualityScores,
306 final List<? extends RadioSourceLocated<Point3D>> sources,
307 final RobustRangingPositionEstimatorListener<Point3D> listener) {
308 this(sources, listener);
309 internalSetSourceQualityScores(sourceQualityScores);
310 internalSetFingerprintReadingsQualityScores(fingerprintReadingsQualityScores);
311 }
312
313 /**
314 * Constructor.
315 *
316 * @param sourceQualityScores quality scores corresponding to
317 * each provided located radio source.
318 * The larger the score value the better
319 * the quality of the radio source.
320 * @param fingerprintReadingsQualityScores quality scores corresponding to readings
321 * within provided fingerprint. The larger
322 * the score the better the quality of the
323 * reading.
324 * @param fingerprint fingerprint containing ranging readings
325 * at an unknown location for provided
326 * location radio sources.
327 * @param listener listener in charge of handling events.
328 * @throws IllegalArgumentException if provided fingerprint is null.
329 */
330 public PROMedSRobustRangingPositionEstimator3D(
331 final double[] sourceQualityScores, final double[] fingerprintReadingsQualityScores,
332 final RangingFingerprint<? extends RadioSource, ? extends RangingReading<?
333 extends RadioSource>> fingerprint,
334 final RobustRangingPositionEstimatorListener<Point3D> listener) {
335 this(fingerprint, listener);
336 internalSetSourceQualityScores(sourceQualityScores);
337 internalSetFingerprintReadingsQualityScores(fingerprintReadingsQualityScores);
338 }
339
340 /**
341 * Constructor.
342 *
343 * @param sourceQualityScores quality scores corresponding to
344 * each provided located radio source.
345 * The larger the score value the better
346 * the quality of the radio source.
347 * @param fingerprintReadingsQualityScores quality scores corresponding to readings
348 * within provided fingerprint. The larger
349 * the score the better the quality of the
350 * reading.
351 * @param sources located radio sources used for
352 * lateration.
353 * @param fingerprint fingerprint containing ranging readings
354 * at an unknown location for provided
355 * located radio sources.
356 * @param listener listener in charge of handling events.
357 * @throws IllegalArgumentException if either provided sources or fingerprint is
358 * null or the number of provided sources is less than the required minimum.
359 */
360 public PROMedSRobustRangingPositionEstimator3D(
361 final double[] sourceQualityScores, final double[] fingerprintReadingsQualityScores,
362 final List<? extends RadioSourceLocated<Point3D>> sources,
363 final RangingFingerprint<? extends RadioSource, ? extends RangingReading<?
364 extends RadioSource>> fingerprint,
365 final RobustRangingPositionEstimatorListener<Point3D> listener) {
366 this(sources, fingerprint, listener);
367 internalSetSourceQualityScores(sourceQualityScores);
368 internalSetFingerprintReadingsQualityScores(fingerprintReadingsQualityScores);
369 }
370
371 /**
372 * Returns quality scores corresponding to each radio source.
373 * The larger the score value the better the quality of the sample.
374 *
375 * @return quality scores corresponding to each radio source.
376 */
377 @Override
378 public double[] getSourceQualityScores() {
379 return sourceQualityScores;
380 }
381
382 /**
383 * Sets quality scores corresponding to each radio source.
384 * The larger the score value the better the quality of the radio source.
385 *
386 * @param sourceQualityScores quality scores corresponding to each radio source.
387 * @throws LockedException if this instance is locked.
388 * @throws IllegalArgumentException if provided quality scores length is smaller
389 * than minimum required samples.
390 */
391 @Override
392 public void setSourceQualityScores(final double[] sourceQualityScores) throws LockedException {
393 if (isLocked()) {
394 throw new LockedException();
395 }
396 internalSetSourceQualityScores(sourceQualityScores);
397 }
398
399 /**
400 * Gets quality scores corresponding to each reading within provided fingerprint.
401 * The larger the score value the better the quality of the reading.
402 * This implementation always returns null.
403 * Subclasses using quality scores must implement proper behavior.
404 *
405 * @return quality scores corresponding to each reading within provided
406 * fingerprint.
407 */
408 @Override
409 public double[] getFingerprintReadingsQualityScores() {
410 return fingerprintReadingsQualityScores;
411 }
412
413 /**
414 * Sets quality scores corresponding to each reading within provided fingerprint.
415 * The larger the score value the better the quality of the reading.
416 * This implementation makes no action.
417 * Subclasses using quality scores must implement proper behavior.
418 *
419 * @param fingerprintReadingsQualityScores quality scores corresponding to each
420 * reading within provided fingerprint.
421 * @throws LockedException if this instance is locked.
422 * @throws IllegalArgumentException if provided quality scores length is smaller
423 * than minimum required samples.
424 */
425 @Override
426 public void setFingerprintReadingsQualityScores(final double[] fingerprintReadingsQualityScores)
427 throws LockedException {
428 if (isLocked()) {
429 throw new LockedException();
430 }
431 internalSetFingerprintReadingsQualityScores(fingerprintReadingsQualityScores);
432 }
433
434 /**
435 * Returns threshold to be used to keep the algorithm iterating in case that
436 * best estimated threshold using median of residuals is not small enough.
437 * Once a solution is found that generates a threshold below this value, the
438 * algorithm will stop.
439 * The stop threshold can be used to prevent the LMedS algorithm to iterate
440 * too many times in cases where samples have a very similar accuracy.
441 * For instance, in cases where proportion of outliers is very small (close
442 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
443 * iterate for a long time trying to find the best solution when indeed
444 * there is no need to do that if a reasonable threshold has already been
445 * reached.
446 * Because of this behaviour the stop threshold can be set to a value much
447 * lower than the one typically used in RANSAC, and yet the algorithm could
448 * still produce even smaller thresholds in estimated results.
449 *
450 * @return stop threshold to stop the algorithm prematurely when a certain
451 * accuracy has been reached.
452 */
453 public double getStopThreshold() {
454 return ((PROMedSRobustLateration3DSolver) laterationSolver).getStopThreshold();
455 }
456
457 /**
458 * Sets threshold to be used to keep the algorithm iterating in case that
459 * best estimated threshold using median of residuals is not small enough.
460 * Once a solution is found that generates a threshold below this value,
461 * the algorithm will stop.
462 * The stop threshold can be used to prevent the LMedS algorithm to iterate
463 * too many times in cases where samples have a very similar accuracy.
464 * For instance, in cases where proportion of outliers is very small (close
465 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
466 * iterate for a long time trying to find the best solution when indeed
467 * there is no need to do that if a reasonable threshold has already been
468 * reached.
469 * Because of this behaviour the stop threshold can be set to a value much
470 * lower than the one typically used in RANSAC, and yet the algorithm could
471 * still produce even smaller thresholds in estimated results.
472 *
473 * @param stopThreshold stop threshold to stop the algorithm prematurely
474 * when a certain accuracy has been reached.
475 * @throws IllegalArgumentException if provided value is zero or negative.
476 * @throws LockedException if this solver is locked.
477 */
478 public void setStopThreshold(final double stopThreshold) throws LockedException {
479 ((PROMedSRobustLateration3DSolver) laterationSolver).setStopThreshold(stopThreshold);
480 }
481
482 /**
483 * Returns method being used for robust estimation.
484 *
485 * @return method being used for robust estimation.
486 */
487 @Override
488 public RobustEstimatorMethod getMethod() {
489 return RobustEstimatorMethod.PROMEDS;
490 }
491
492 /**
493 * Initializes robust lateration solver.
494 */
495 private void init() {
496 laterationSolver = new PROMedSRobustLateration3DSolver(trilaterationSolverListener);
497 }
498
499 /**
500 * Sets quality scores corresponding to each provided located radio source.
501 * This method is used internally and does not check whether instance is
502 * locked or not.
503 *
504 * @param sourceQualityScores quality scores to be set.
505 * @throws IllegalArgumentException if provided quality scores length
506 * is smaller than 3 samples.
507 */
508 private void internalSetSourceQualityScores(final double[] sourceQualityScores) {
509 if (sourceQualityScores == null || sourceQualityScores.length < getMinRequiredSources()) {
510 throw new IllegalArgumentException();
511 }
512
513 this.sourceQualityScores = sourceQualityScores;
514
515 buildPositionsDistancesDistanceStandardDeviationsAndQualityScores();
516 }
517
518 /**
519 * Sets quality scores corresponding to each provided reading within provided
520 * fingerprint.
521 * This method is used internally and does not check whether instance is locked
522 * or not.
523 *
524 * @param fingerprintReadingsQualityScores quality scores to be set.
525 * @throws IllegalArgumentException if provided quality scores length is
526 * smaller than 3 samples.
527 */
528 private void internalSetFingerprintReadingsQualityScores(final double[] fingerprintReadingsQualityScores) {
529 if (fingerprintReadingsQualityScores == null
530 || fingerprintReadingsQualityScores.length < getMinRequiredSources()) {
531 throw new IllegalArgumentException();
532 }
533
534 this.fingerprintReadingsQualityScores = fingerprintReadingsQualityScores;
535
536 buildPositionsDistancesDistanceStandardDeviationsAndQualityScores();
537 }
538 }