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