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