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