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.indoor.radiosource;
17
18 import com.irurueta.geometry.Point3D;
19 import com.irurueta.navigation.LockedException;
20 import com.irurueta.navigation.NotReadyException;
21 import com.irurueta.navigation.indoor.RadioSource;
22 import com.irurueta.navigation.indoor.RangingAndRssiReadingLocated;
23 import com.irurueta.numerical.robust.PROMedSRobustEstimator;
24 import com.irurueta.numerical.robust.PROMedSRobustEstimatorListener;
25 import com.irurueta.numerical.robust.RobustEstimator;
26 import com.irurueta.numerical.robust.RobustEstimatorException;
27 import com.irurueta.numerical.robust.RobustEstimatorMethod;
28
29 import java.util.List;
30
31 /**
32 * Robustly estimate 3D position, transmitted power and path-loss
33 * exponent of a radio source (e.g. Wi-Fi access point or bluetooth beacon), by discarding
34 * outliers using PROMedS algorithm and assuming that the ranging data is available to
35 * obtain position with greater accuracy and that the radio source emits isotropically
36 * following the expression below:
37 * Pr = Pt*Gt*Gr*lambda^2 / (4*pi*d)^2,
38 * where Pr is the received power (expressed in mW),
39 * Gt is the Gain of the transmission antenna
40 * Gr is the Gain of the receiver antenna
41 * d is the distance between emitter and receiver
42 * and lambda is the wavelength and is equal to: lambda = c / f,
43 * where c is the speed of light
44 * and f is the carrier frequency of the radio signal.
45 * Because usually information about the antenna of the radio source cannot be
46 * retrieved (because many measurements are made on unknown devices where
47 * physical access is not possible), this implementation will estimate the
48 * equivalent transmitted power as: Pte = Pt * Gt * Gr.
49 * If Readings contain RSSI standard deviations, those values will be used,
50 * otherwise it will be assumed an RSSI standard deviation of 1 dB.
51 *
52 * @param <S> a {@link RadioSource} type.
53 */
54 @SuppressWarnings("Duplicates")
55 public class PROMedSRobustRangingAndRssiRadioSourceEstimator3D<S extends RadioSource> extends
56 RobustRangingAndRssiRadioSourceEstimator3D<S> {
57
58 /**
59 * Default value to be used for stop threshold. Stop threshold can be used to
60 * avoid keeping the algorithm unnecessarily iterating in case that best
61 * estimated threshold using median of residuals is not small enough. Once a
62 * solution is found that generates a threshold below this value, the
63 * algorithm will stop.
64 * The stop threshold can be used to prevent the LMedS algorithm iterating
65 * too many times in cases where samples have a very similar accuracy.
66 * For instance, in cases where proportion of outliers is very small (close
67 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
68 * iterate for a long time trying to find the best solution when indeed
69 * there is no need to do that if a reasonable threshold has already been
70 * reached.
71 * Because of this behaviour the stop threshold can be set to a value much
72 * lower than the one typically used in RANSAC, and yet the algorithm could
73 * still produce even smaller thresholds in estimated results.
74 */
75 public static final double DEFAULT_STOP_THRESHOLD = 1e-4;
76
77 /**
78 * Minimum allowed stop threshold value.
79 */
80 public static final double MIN_STOP_THRESHOLD = 0.0;
81
82 /**
83 * Threshold to be used to keep the algorithm iterating in case that best
84 * estimated threshold using median of residuals is not small enough. Once
85 * a solution is found that generates a threshold below this value, the
86 * algorithm will stop.
87 * The stop threshold can be used to prevent the LMedS algorithm iterating
88 * too many times in cases where samples have a very similar accuracy.
89 * For instance, in cases where proportion of outliers is very small (close
90 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
91 * iterate for a long time trying to find the best solution when indeed
92 * there is no need to do that if a reasonable threshold has already been
93 * reached.
94 * Because of this behaviour the stop threshold can be set to a value much
95 * lower than the one typically used in RANSAC, and yet the algorithm could
96 * still produce even smaller thresholds in estimated results.
97 */
98 private double stopThreshold = DEFAULT_STOP_THRESHOLD;
99
100 /**
101 * Quality scores corresponding to each provided sample.
102 * The larger the score value the better the quality of the sample.
103 */
104 private double[] qualityScores;
105
106 /**
107 * Constructor.
108 */
109 public PROMedSRobustRangingAndRssiRadioSourceEstimator3D() {
110 super();
111 }
112
113 /**
114 * Constructor.
115 * Sets signal readings belonging to the same radio source.
116 *
117 * @param readings signal readings belonging to the same radio source.
118 * @throws IllegalArgumentException if readings are not valid.
119 */
120 public PROMedSRobustRangingAndRssiRadioSourceEstimator3D(
121 final List<? extends RangingAndRssiReadingLocated<S, Point3D>> readings) {
122 super(readings);
123 }
124
125 /**
126 * Constructor.
127 *
128 * @param listener listener in charge of attending events raised by this instance.
129 */
130 public PROMedSRobustRangingAndRssiRadioSourceEstimator3D(
131 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point3D> listener) {
132 this.listener = listener;
133 }
134
135 /**
136 * Constructor.
137 * Sets signal readings belonging to the same radio source.
138 *
139 * @param readings signal readings belonging to the same radio source.
140 * @param listener listener in charge of attending events raised by this instance.
141 * @throws IllegalArgumentException if readings are not valid.
142 */
143 public PROMedSRobustRangingAndRssiRadioSourceEstimator3D(
144 final List<? extends RangingAndRssiReadingLocated<S, Point3D>> readings,
145 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point3D> listener) {
146 super(readings, listener);
147 }
148
149 /**
150 * Constructor.
151 * Sets signal readings belonging to the same radio source.
152 *
153 * @param readings signal readings belonging to the same radio source.
154 * @param initialPosition initial position to start the estimation of access
155 * point position.
156 * @throws IllegalArgumentException if readings are not valid.
157 */
158 public PROMedSRobustRangingAndRssiRadioSourceEstimator3D(
159 final List<? extends RangingAndRssiReadingLocated<S, Point3D>> readings, final Point3D initialPosition) {
160 super(readings, initialPosition);
161 }
162
163 /**
164 * Constructor.
165 *
166 * @param initialPosition initial position to start the estimation of access
167 * point position.
168 */
169 public PROMedSRobustRangingAndRssiRadioSourceEstimator3D(final Point3D initialPosition) {
170 super(initialPosition);
171 }
172
173 /**
174 * Constructor.
175 *
176 * @param initialPosition initial position to start the estimation of access
177 * point position.
178 * @param listener listener in charge of attending events raised by this instance.
179 */
180 public PROMedSRobustRangingAndRssiRadioSourceEstimator3D(
181 final Point3D initialPosition,
182 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point3D> listener) {
183 super(initialPosition, listener);
184 }
185
186 /**
187 * Constructor.
188 * Sets signal readings belonging to the same radio source.
189 *
190 * @param readings signal readings belonging to the same radio source.
191 * @param initialPosition initial position to start the estimation of radio
192 * source position.
193 * @param listener listener in charge of attending events raised by this instance.
194 * @throws IllegalArgumentException if readings are not valid.
195 */
196 public PROMedSRobustRangingAndRssiRadioSourceEstimator3D(
197 final List<? extends RangingAndRssiReadingLocated<S, Point3D>> readings, final Point3D initialPosition,
198 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point3D> listener) {
199 super(readings, initialPosition, listener);
200 }
201
202 /**
203 * Constructor.
204 *
205 * @param initialTransmittedPowerdBm initial transmitted power to start the
206 * estimation of radio source transmitted power
207 * (expressed in dBm's)
208 */
209 public PROMedSRobustRangingAndRssiRadioSourceEstimator3D(final Double initialTransmittedPowerdBm) {
210 super(initialTransmittedPowerdBm);
211 }
212
213 /**
214 * Constructor.
215 * Sets signal readings belonging to the same radio source.
216 *
217 * @param readings signal readings belonging to the same radio source.
218 * @param initialTransmittedPowerdBm initial transmitted power to start the
219 * estimation of radio source transmitted power
220 * (expressed in dBm's)
221 * @throws IllegalArgumentException if readings are not valid.
222 */
223 public PROMedSRobustRangingAndRssiRadioSourceEstimator3D(
224 final List<? extends RangingAndRssiReadingLocated<S, Point3D>> readings,
225 final Double initialTransmittedPowerdBm) {
226 super(readings, initialTransmittedPowerdBm);
227 }
228
229 /**
230 * Constructor.
231 *
232 * @param initialTransmittedPowerdBm initial transmitted power to start the
233 * estimation of radio source transmitted power
234 * (expressed in dBm's)
235 * @param listener listener in charge of attending events raised by this instance.
236 */
237 public PROMedSRobustRangingAndRssiRadioSourceEstimator3D(
238 final Double initialTransmittedPowerdBm,
239 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point3D> listener) {
240 super(initialTransmittedPowerdBm, listener);
241 }
242
243 /**
244 * Constructor.
245 * Sets signal readings belonging to the same radio source.
246 *
247 * @param readings signal readings belonging to the same radio source.
248 * @param initialTransmittedPowerdBm initial transmitted power to start the
249 * estimation of radio source transmitted power
250 * (expressed in dBm's)
251 * @param listener listener in charge of attending events raised by this instance.
252 * @throws IllegalArgumentException if readings are not valid.
253 */
254 public PROMedSRobustRangingAndRssiRadioSourceEstimator3D(
255 final List<? extends RangingAndRssiReadingLocated<S, Point3D>> readings,
256 final Double initialTransmittedPowerdBm,
257 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point3D> listener) {
258 super(readings, initialTransmittedPowerdBm, listener);
259 }
260
261 /**
262 * Constructor.
263 * Sets signal readings belonging to the same radio source.
264 *
265 * @param readings signal readings belonging to the same radio source.
266 * @param initialPosition initial position to start the estimation of access
267 * point position.
268 * @param initialTransmittedPowerdBm initial transmitted power to start the
269 * estimation of radio source transmitted power
270 * (expressed in dBm's).
271 * @throws IllegalArgumentException if readings are not valid.
272 */
273 public PROMedSRobustRangingAndRssiRadioSourceEstimator3D(
274 final List<? extends RangingAndRssiReadingLocated<S, Point3D>> readings, final Point3D initialPosition,
275 final Double initialTransmittedPowerdBm) {
276 super(readings, initialPosition, initialTransmittedPowerdBm);
277 }
278
279 /**
280 * Constructor.
281 *
282 * @param initialPosition initial position to start the estimation of radio
283 * source position.
284 * @param initialTransmittedPowerdBm initial transmitted power to start the
285 * estimation of radio source transmitted power
286 * (expressed in dBm's).
287 */
288 public PROMedSRobustRangingAndRssiRadioSourceEstimator3D(
289 final Point3D initialPosition, final Double initialTransmittedPowerdBm) {
290 super(initialPosition, initialTransmittedPowerdBm);
291 }
292
293 /**
294 * Constructor.
295 *
296 * @param initialPosition initial position to start the estimation of radio
297 * source position.
298 * @param initialTransmittedPowerdBm initial transmitted power to start the
299 * estimation of radio source transmitted power
300 * (expressed in dBm's).
301 * @param listener in charge of attending events raised by this instance.
302 */
303 public PROMedSRobustRangingAndRssiRadioSourceEstimator3D(
304 final Point3D initialPosition, final Double initialTransmittedPowerdBm,
305 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point3D> listener) {
306 super(initialPosition, initialTransmittedPowerdBm, listener);
307 }
308
309 /**
310 * Constructor.
311 * Sets signal readings belonging to the same radio source.
312 *
313 * @param readings Wi-Fi signal readings belonging to the same radio source.
314 * @param initialPosition initial position to start the estimation of radio
315 * source position.
316 * @param initialTransmittedPowerdBm initial transmitted power to start the
317 * estimation of radio source transmitted power
318 * (expressed in dBm's).
319 * @param listener listener in charge of attending events raised by this instance.
320 * @throws IllegalArgumentException if readings are not valid.
321 */
322 public PROMedSRobustRangingAndRssiRadioSourceEstimator3D(
323 final List<? extends RangingAndRssiReadingLocated<S, Point3D>> readings, final Point3D initialPosition,
324 final Double initialTransmittedPowerdBm,
325 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point3D> listener) {
326 super(readings, initialPosition, initialTransmittedPowerdBm, listener);
327 }
328
329 /**
330 * Constructor.
331 * Sets signal readings belonging to the same radio source.
332 *
333 * @param readings signal readings belonging to the same radio source.
334 * @param initialPosition initial position to start the estimation of radio
335 * source position.
336 * @param initialTransmittedPowerdBm initial transmitted power to start the
337 * estimation of radio source transmitted power
338 * (expressed in dBm's).
339 * @param initialPathLossExponent initial path loss exponent. A typical value is 2.0.
340 * @throws IllegalArgumentException if readings are not valid.
341 */
342 public PROMedSRobustRangingAndRssiRadioSourceEstimator3D(
343 final List<? extends RangingAndRssiReadingLocated<S, Point3D>> readings, final Point3D initialPosition,
344 final Double initialTransmittedPowerdBm, final double initialPathLossExponent) {
345 super(readings, initialPosition, initialTransmittedPowerdBm, initialPathLossExponent);
346 }
347
348 /**
349 * Constructor.
350 *
351 * @param initialPosition initial position to start the estimation of radio
352 * source position.
353 * @param initialTransmittedPowerdBm initial transmitted power to start the
354 * estimation of radio source transmitted power
355 * (expressed in dBm's).
356 * @param initialPathLossExponent initial path loss exponent. A typical value is 2.0.
357 */
358 public PROMedSRobustRangingAndRssiRadioSourceEstimator3D(
359 final Point3D initialPosition, final Double initialTransmittedPowerdBm,
360 final double initialPathLossExponent) {
361 super(initialPosition, initialTransmittedPowerdBm, initialPathLossExponent);
362 }
363
364 /**
365 * Constructor.
366 *
367 * @param initialPosition initial position to start the estimation of radio
368 * source position.
369 * @param initialTransmittedPowerdBm initial transmitted power to start the
370 * estimation of radio source transmitted power
371 * (expressed in dBm's).
372 * @param initialPathLossExponent initial path loss exponent. A typical value is 2.0.
373 * @param listener listener in charge of attending events raised by this instance.
374 */
375 public PROMedSRobustRangingAndRssiRadioSourceEstimator3D(
376 final Point3D initialPosition, final Double initialTransmittedPowerdBm,
377 final double initialPathLossExponent,
378 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point3D> listener) {
379 super(initialPosition, initialTransmittedPowerdBm, initialPathLossExponent, listener);
380 }
381
382 /**
383 * Constructor.
384 * Sets signal readings belonging to the same radio source.
385 *
386 * @param readings signal readings belonging to the same radio source.
387 * @param initialPosition initial position to start the estimation of radio
388 * source position.
389 * @param initialTransmittedPowerdBm initial transmitted power to start the
390 * estimation of radio source transmitted power
391 * (expressed in dBm's).
392 * @param initialPathLossExponent initial path loss exponent. A typical value is 2.0.
393 * @param listener listener in charge of attending events raised by this instance.
394 * @throws IllegalArgumentException if readings are not valid.
395 */
396 public PROMedSRobustRangingAndRssiRadioSourceEstimator3D(
397 final List<? extends RangingAndRssiReadingLocated<S, Point3D>> readings, final Point3D initialPosition,
398 final Double initialTransmittedPowerdBm, final double initialPathLossExponent,
399 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point3D> listener) {
400 super(readings, initialPosition, initialTransmittedPowerdBm, initialPathLossExponent, listener);
401 }
402
403 /**
404 * Constructor.
405 *
406 * @param qualityScores quality scores corresponding to each provided
407 * sample. The larger the score value the better
408 * the quality of the sample.
409 * @throws IllegalArgumentException if quality scores is null, or length
410 * of quality scores is less than required minimum.
411 */
412 public PROMedSRobustRangingAndRssiRadioSourceEstimator3D(final double[] qualityScores) {
413 super();
414 internalSetQualityScores(qualityScores);
415 }
416
417 /**
418 * Constructor.
419 * Sets signal readings belonging to the same radio source.
420 *
421 * @param qualityScores quality scores corresponding to each provided
422 * sample. The larger the score value the better
423 * the quality of the sample.
424 * @param readings signal readings belonging to the same radio source.
425 * @throws IllegalArgumentException if readings are not valid, quality scores
426 * is null, or length of quality scores is less than required minimum.
427 */
428 public PROMedSRobustRangingAndRssiRadioSourceEstimator3D(
429 final double[] qualityScores, final List<? extends RangingAndRssiReadingLocated<S, Point3D>> readings) {
430 super(readings);
431 internalSetQualityScores(qualityScores);
432 }
433
434 /**
435 * Constructor.
436 *
437 * @param qualityScores quality scores corresponding to each provided
438 * sample. The larger the score value the better
439 * the quality of the sample.
440 * @param listener listener in charge of attending events raised by this instance.
441 * @throws IllegalArgumentException if quality scores is null, or length
442 * of quality scores is less than required minimum.
443 */
444 public PROMedSRobustRangingAndRssiRadioSourceEstimator3D(
445 final double[] qualityScores, final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point3D> listener) {
446 super(listener);
447 internalSetQualityScores(qualityScores);
448 }
449
450 /**
451 * Constructor.
452 * Sets signal readings belonging to the same radio source.
453 *
454 * @param qualityScores quality scores corresponding to each provided
455 * sample. The larger the score value the better
456 * the quality of the sample.
457 * @param readings signal readings belonging to the same radio source.
458 * @param listener listener in charge of attending events raised by this instance.
459 * @throws IllegalArgumentException if readings are not valid, quality scores
460 * is null, or length of quality scores is less than required minimum.
461 */
462 public PROMedSRobustRangingAndRssiRadioSourceEstimator3D(
463 final double[] qualityScores, final List<? extends RangingAndRssiReadingLocated<S, Point3D>> readings,
464 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point3D> listener) {
465 super(readings, listener);
466 internalSetQualityScores(qualityScores);
467 }
468
469 /**
470 * Constructor.
471 * Sets signal readings belonging to the same radio source.
472 *
473 * @param qualityScores quality scores corresponding to each provided
474 * sample. The larger the score value the better
475 * the quality of the sample.
476 * @param readings signal readings belonging to the same radio source.
477 * @param initialPosition initial position to start the estimation of radio
478 * source position.
479 * @throws IllegalArgumentException if readings are not valid, quality scores
480 * is null, or length of quality scores is less than required minimum.
481 */
482 public PROMedSRobustRangingAndRssiRadioSourceEstimator3D(
483 final double[] qualityScores, final List<? extends RangingAndRssiReadingLocated<S, Point3D>> readings,
484 final Point3D initialPosition) {
485 super(readings, initialPosition);
486 internalSetQualityScores(qualityScores);
487 }
488
489 /**
490 * Constructor.
491 *
492 * @param qualityScores quality scores corresponding to each provided
493 * sample. The larger the score value the better
494 * the quality of the sample.
495 * @param initialPosition initial position to start the estimation of radio
496 * source position.
497 */
498 public PROMedSRobustRangingAndRssiRadioSourceEstimator3D(
499 final double[] qualityScores, final Point3D initialPosition) {
500 super(initialPosition);
501 internalSetQualityScores(qualityScores);
502 }
503
504 /**
505 * Constructor.
506 *
507 * @param qualityScores quality scores corresponding to each provided
508 * sample. The larger the score value the better
509 * the quality of the sample.
510 * @param initialPosition initial position to start the estimation of radio
511 * source position.
512 * @param listener listener in charge of attending events raised by this instance.
513 * @throws IllegalArgumentException if quality scores is null, or length
514 * of quality scores is less than required minimum.
515 */
516 public PROMedSRobustRangingAndRssiRadioSourceEstimator3D(
517 final double[] qualityScores, final Point3D initialPosition,
518 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point3D> listener) {
519 super(initialPosition, listener);
520 internalSetQualityScores(qualityScores);
521 }
522
523 /**
524 * Constructor.
525 * Sets signal readings belonging to the same radio source.
526 *
527 * @param qualityScores quality scores corresponding to each provided
528 * sample. The larger the score value the better
529 * the quality of the sample.
530 * @param readings signal readings belonging to the same radio source.
531 * @param initialPosition initial position to start the estimation of radio
532 * source position.
533 * @param listener listener in charge of attending events raised by this instance.
534 * @throws IllegalArgumentException if readings are not valid, quality scores
535 * is null, or length of quality scores is less than required minimum.
536 */
537 public PROMedSRobustRangingAndRssiRadioSourceEstimator3D(
538 final double[] qualityScores, final List<? extends RangingAndRssiReadingLocated<S, Point3D>> readings,
539 final Point3D initialPosition,
540 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point3D> listener) {
541 super(readings, initialPosition, listener);
542 internalSetQualityScores(qualityScores);
543 }
544
545 /**
546 * Constructor.
547 *
548 * @param qualityScores quality scores corresponding to each provided
549 * sample. The larger the score value the better
550 * the quality of the sample.
551 * @param initialTransmittedPowerdBm initial transmitted power to start the
552 * estimation of radio source transmitted power
553 * (expressed in dBm's)
554 * @throws IllegalArgumentException if quality scores is null, or length
555 * of quality scores is less than required minimum.
556 */
557 public PROMedSRobustRangingAndRssiRadioSourceEstimator3D(
558 final double[] qualityScores, final Double initialTransmittedPowerdBm) {
559 super(initialTransmittedPowerdBm);
560 internalSetQualityScores(qualityScores);
561 }
562
563 /**
564 * Constructor.
565 * Sets signal readings belonging to the same radio source.
566 *
567 * @param qualityScores quality scores corresponding to each provided
568 * sample. The larger the score value the better
569 * the quality of the sample.
570 * @param readings signal readings belonging to the same radio source.
571 * @param initialTransmittedPowerdBm initial transmitted power to start the
572 * estimation of radio source transmitted power
573 * (expressed in dBm's)
574 * @throws IllegalArgumentException if readings are not valid, quality scores
575 * is null, or length of quality scores is less than required minimum.
576 */
577 public PROMedSRobustRangingAndRssiRadioSourceEstimator3D(
578 final double[] qualityScores, final List<? extends RangingAndRssiReadingLocated<S, Point3D>> readings,
579 final Double initialTransmittedPowerdBm) {
580 super(readings, initialTransmittedPowerdBm);
581 internalSetQualityScores(qualityScores);
582 }
583
584 /**
585 * Constructor.
586 *
587 * @param qualityScores quality scores corresponding to each provided
588 * sample. The larger the score value the better
589 * the quality of the sample.
590 * @param initialTransmittedPowerdBm initial transmitted power to start the
591 * estimation of radio source transmitted power
592 * (expressed in dBm's)
593 * @param listener listener in charge of attending events raised by this instance.
594 * @throws IllegalArgumentException if quality scores is null, or length
595 * of quality scores is less than required minimum.
596 */
597 public PROMedSRobustRangingAndRssiRadioSourceEstimator3D(
598 final double[] qualityScores, final Double initialTransmittedPowerdBm,
599 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point3D> listener) {
600 super(initialTransmittedPowerdBm, listener);
601 internalSetQualityScores(qualityScores);
602 }
603
604 /**
605 * Constructor.
606 * Sets signal readings belonging to the same radio source.
607 *
608 * @param qualityScores quality scores corresponding to each provided
609 * sample. The larger the score value the better
610 * the quality of the sample.
611 * @param readings signal readings belonging to the same radio source.
612 * @param initialTransmittedPowerdBm initial transmitted power to start the
613 * estimation of radio source transmitted power
614 * (expressed in dBm's)
615 * @param listener listener in charge of attending events raised by this instance.
616 * @throws IllegalArgumentException if readings are not valid, quality scores
617 * is null, or length of quality scores is less than required minimum.
618 */
619 public PROMedSRobustRangingAndRssiRadioSourceEstimator3D(
620 final double[] qualityScores, final List<? extends RangingAndRssiReadingLocated<S, Point3D>> readings,
621 final Double initialTransmittedPowerdBm,
622 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point3D> listener) {
623 super(readings, initialTransmittedPowerdBm, listener);
624 internalSetQualityScores(qualityScores);
625 }
626
627 /**
628 * Constructor.
629 * Sets signal readings belonging to the same radio source.
630 *
631 * @param qualityScores quality scores corresponding to each provided
632 * sample. The larger the score value the better
633 * the quality of the sample.
634 * @param readings signal readings belonging to the same radio source.
635 * @param initialPosition initial position to start the estimation of radio
636 * source position.
637 * @param initialTransmittedPowerdBm initial transmitted power to start the
638 * estimation of radio source transmitted power
639 * (expressed in dBm's).
640 * @throws IllegalArgumentException if readings are not valid, quality scores
641 * is null, or length of quality scores is less than required minimum.
642 */
643 public PROMedSRobustRangingAndRssiRadioSourceEstimator3D(
644 final double[] qualityScores, final List<? extends RangingAndRssiReadingLocated<S, Point3D>> readings,
645 final Point3D initialPosition, final Double initialTransmittedPowerdBm) {
646 super(readings, initialPosition, initialTransmittedPowerdBm);
647 internalSetQualityScores(qualityScores);
648 }
649
650 /**
651 * Constructor.
652 *
653 * @param qualityScores quality scores corresponding to each provided
654 * sample. The larger the score value the better
655 * the quality of the sample.
656 * @param initialPosition initial position to start the estimation of radio
657 * source position.
658 * @param initialTransmittedPowerdBm initial transmitted power to start the
659 * estimation of radio source transmitted power
660 * (expressed in dBm's).
661 * @throws IllegalArgumentException if quality scores is null, or length
662 * of quality scores is less than required minimum.
663 */
664 public PROMedSRobustRangingAndRssiRadioSourceEstimator3D(
665 final double[] qualityScores, final Point3D initialPosition, final Double initialTransmittedPowerdBm) {
666 super(initialPosition, initialTransmittedPowerdBm);
667 internalSetQualityScores(qualityScores);
668 }
669
670 /**
671 * Constructor.
672 *
673 * @param qualityScores quality scores corresponding to each provided
674 * sample. The larger the score value the better
675 * the quality of the sample.
676 * @param initialPosition initial position to start the estimation of radio
677 * source position.
678 * @param initialTransmittedPowerdBm initial transmitted power to start the
679 * estimation of radio source transmitted power
680 * (expressed in dBm's).
681 * @param listener in charge of attending events raised by this instance.
682 * @throws IllegalArgumentException if quality scores is null, or length
683 * of quality scores is less than required minimum.
684 */
685 public PROMedSRobustRangingAndRssiRadioSourceEstimator3D(
686 final double[] qualityScores, final Point3D initialPosition, final Double initialTransmittedPowerdBm,
687 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point3D> listener) {
688 super(initialPosition, initialTransmittedPowerdBm, listener);
689 internalSetQualityScores(qualityScores);
690 }
691
692 /**
693 * Constructor.
694 * Sets signal readings belonging to the same radio source.
695 *
696 * @param qualityScores quality scores corresponding to each provided
697 * sample. The larger the score value the better
698 * the quality of the sample.
699 * @param readings signal readings belonging to the same radio source.
700 * @param initialPosition initial position to start the estimation of radio
701 * source position.
702 * @param initialTransmittedPowerdBm initial transmitted power to start the
703 * estimation of radio source transmitted power
704 * (expressed in dBm's).
705 * @param listener listener in charge of attending events raised by this instance.
706 * @throws IllegalArgumentException if readings are not valid, quality scores
707 * is null, or length of quality scores is less than required minimum.
708 */
709 public PROMedSRobustRangingAndRssiRadioSourceEstimator3D(
710 final double[] qualityScores, final List<? extends RangingAndRssiReadingLocated<S, Point3D>> readings,
711 final Point3D initialPosition, final Double initialTransmittedPowerdBm,
712 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point3D> listener) {
713 super(readings, initialPosition, initialTransmittedPowerdBm, listener);
714 internalSetQualityScores(qualityScores);
715 }
716
717 /**
718 * Constructor.
719 * Sets signal readings belonging to the same radio source.
720 *
721 * @param qualityScores quality scores corresponding to each provided
722 * sample. The larger the score value the better
723 * the quality of the sample.
724 * @param readings signal readings belonging to the same radio source.
725 * @param initialPosition initial position to start the estimation of radio
726 * source position.
727 * @param initialTransmittedPowerdBm initial transmitted power to start the
728 * estimation of radio source transmitted power
729 * (expressed in dBm's).
730 * @param initialPathLossExponent initial path loss exponent. A typical value is 2.0.
731 * @throws IllegalArgumentException if readings are not valid, quality scores
732 * is null, or length of quality scores is less than required minimum.
733 */
734 public PROMedSRobustRangingAndRssiRadioSourceEstimator3D(
735 final double[] qualityScores, final List<? extends RangingAndRssiReadingLocated<S, Point3D>> readings,
736 final Point3D initialPosition, final Double initialTransmittedPowerdBm,
737 final double initialPathLossExponent) {
738 super(readings, initialPosition, initialTransmittedPowerdBm, initialPathLossExponent);
739 internalSetQualityScores(qualityScores);
740 }
741
742 /**
743 * Constructor.
744 *
745 * @param qualityScores quality scores corresponding to each provided
746 * sample. The larger the score value the better
747 * the quality of the sample.
748 * @param initialPosition initial position to start the estimation of radio
749 * source position.
750 * @param initialTransmittedPowerdBm initial transmitted power to start the
751 * estimation of radio source transmitted power
752 * (expressed in dBm's).
753 * @param initialPathLossExponent initial path loss exponent. A typical value is 2.0.
754 */
755 public PROMedSRobustRangingAndRssiRadioSourceEstimator3D(
756 final double[] qualityScores, final Point3D initialPosition, final Double initialTransmittedPowerdBm,
757 final double initialPathLossExponent) {
758 super(initialPosition, initialTransmittedPowerdBm, initialPathLossExponent);
759 internalSetQualityScores(qualityScores);
760 }
761
762 /**
763 * Constructor.
764 *
765 * @param qualityScores quality scores corresponding to each provided
766 * sample. The larger the score value the better
767 * the quality of the sample.
768 * @param initialPosition initial position to start the estimation of radio
769 * source position.
770 * @param initialTransmittedPowerdBm initial transmitted power to start the
771 * estimation of radio source transmitted power
772 * (expressed in dBm's).
773 * @param initialPathLossExponent initial path loss exponent. A typical value is 2.0.
774 * @param listener listener in charge of attending events raised by this instance.
775 */
776 public PROMedSRobustRangingAndRssiRadioSourceEstimator3D(
777 final double[] qualityScores, final Point3D initialPosition, final Double initialTransmittedPowerdBm,
778 final double initialPathLossExponent,
779 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point3D> listener) {
780 super(initialPosition, initialTransmittedPowerdBm, initialPathLossExponent, listener);
781 internalSetQualityScores(qualityScores);
782 }
783
784 /**
785 * Constructor.
786 * Sets signal readings belonging to the same radio source.
787 *
788 * @param qualityScores quality scores corresponding to each provided
789 * sample. The larger the score value the better
790 * the quality of the sample.
791 * @param readings signal readings belonging to the same radio source.
792 * @param initialPosition initial position to start the estimation of radio
793 * source position.
794 * @param initialTransmittedPowerdBm initial transmitted power to start the
795 * estimation of radio source transmitted power
796 * (expressed in dBm's).
797 * @param initialPathLossExponent initial path loss exponent. A typical value is 2.0.
798 * @param listener listener in charge of attending events raised by this instance.
799 * @throws IllegalArgumentException if readings are not valid, quality scores
800 * is null, or length of quality scores is less than required minimum.
801 */
802 public PROMedSRobustRangingAndRssiRadioSourceEstimator3D(
803 final double[] qualityScores, final List<? extends RangingAndRssiReadingLocated<S, Point3D>> readings,
804 final Point3D initialPosition, final Double initialTransmittedPowerdBm,
805 final double initialPathLossExponent,
806 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point3D> listener) {
807 super(readings, initialPosition, initialTransmittedPowerdBm, initialPathLossExponent, listener);
808 internalSetQualityScores(qualityScores);
809 }
810
811 /**
812 * Returns threshold to be used to keep the algorithm iterating in case that
813 * best estimated threshold using median of residuals is not small enough.
814 * Once a solution is found that generates a threshold below this value, the
815 * algorithm will stop.
816 * The stop threshold can be used to prevent the LMedS algorithm to iterate
817 * too many times in cases where samples have a very similar accuracy.
818 * For instance, in cases where proportion of outliers is very small (close
819 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
820 * iterate for a long time trying to find the best solution when indeed
821 * there is no need to do that if a reasonable threshold has already been
822 * reached.
823 * Because of this behaviour the stop threshold can be set to a value much
824 * lower than the one typically used in RANSAC, and yet the algorithm could
825 * still produce even smaller thresholds in estimated results.
826 *
827 * @return stop threshold to stop the algorithm prematurely when a certain
828 * accuracy has been reached.
829 */
830 public double getStopThreshold() {
831 return stopThreshold;
832 }
833
834 /**
835 * Sets threshold to be used to keep the algorithm iterating in case that
836 * best estimated threshold using median of residuals is not small enough.
837 * Once a solution is found that generates a threshold below this value,
838 * the algorithm will stop.
839 * The stop threshold can be used to prevent the LMedS algorithm to iterate
840 * too many times in cases where samples have a very similar accuracy.
841 * For instance, in cases where proportion of outliers is very small (close
842 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
843 * iterate for a long time trying to find the best solution when indeed
844 * there is no need to do that if a reasonable threshold has already been
845 * reached.
846 * Because of this behaviour the stop threshold can be set to a value much
847 * lower than the one typically used in RANSAC, and yet the algorithm could
848 * still produce even smaller thresholds in estimated results.
849 *
850 * @param stopThreshold stop threshold to stop the algorithm prematurely
851 * when a certain accuracy has been reached.
852 * @throws IllegalArgumentException if provided value is zero or negative.
853 * @throws LockedException if this solver is locked.
854 */
855 public void setStopThreshold(final double stopThreshold) throws LockedException {
856 if (isLocked()) {
857 throw new LockedException();
858 }
859 if (stopThreshold <= MIN_STOP_THRESHOLD) {
860 throw new IllegalArgumentException();
861 }
862
863 this.stopThreshold = stopThreshold;
864 }
865
866 /**
867 * Returns quality scores corresponding to each pair of
868 * positions and distances (i.e. sample).
869 * The larger the score value the better the quality of the sample.
870 * This implementation always returns null.
871 * Subclasses using quality scores must implement proper behavior.
872 *
873 * @return quality scores corresponding to each sample.
874 */
875 @Override
876 public double[] getQualityScores() {
877 return qualityScores;
878 }
879
880 /**
881 * Sets quality scores corresponding to each pair of positions and
882 * distances (i.e. sample).
883 * The larger the score value the better the quality of the sample.
884 * This implementation makes no action.
885 * Subclasses using quality scores must implement proper behaviour.
886 *
887 * @param qualityScores quality scores corresponding to each pair of
888 * matched points.
889 * @throws IllegalArgumentException if provided quality scores length
890 * is smaller than minimum required samples.
891 * @throws LockedException if robust solver is locked because an
892 * estimation is already in progress.
893 */
894 @Override
895 public void setQualityScores(final double[] qualityScores) throws LockedException {
896 if (isLocked()) {
897 throw new LockedException();
898 }
899 internalSetQualityScores(qualityScores);
900 }
901
902 /**
903 * Indicates whether solver is ready to find a solution.
904 *
905 * @return true if solver is ready, false otherwise.
906 */
907 @Override
908 public boolean isReady() {
909 return super.isReady() && qualityScores != null && qualityScores.length == readings.size();
910 }
911
912 /**
913 * Robustly estimates position, transmitted power and path-loss exponent for a
914 * radio source.
915 *
916 * @throws LockedException if instance is busy during estimation.
917 * @throws NotReadyException if estimator is not ready.
918 * @throws RobustEstimatorException if estimation fails for any reason
919 * (i.e. numerical instability, no solution available, etc).
920 */
921 @Override
922 public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
923 if (isLocked()) {
924 throw new LockedException();
925 }
926 if (!isReady()) {
927 throw new NotReadyException();
928 }
929
930 final var innerEstimator = new PROMedSRobustEstimator<>(
931 new PROMedSRobustEstimatorListener<Solution<Point3D>>() {
932
933 @Override
934 public double[] getQualityScores() {
935 return qualityScores;
936 }
937
938 @Override
939 public double getThreshold() {
940 return stopThreshold;
941 }
942
943 @Override
944 public int getTotalSamples() {
945 return readings.size();
946 }
947
948 @Override
949 public int getSubsetSize() {
950 return Math.max(preliminarySubsetSize, getMinReadings());
951 }
952
953 @Override
954 public void estimatePreliminarSolutions(
955 final int[] samplesIndices, final List<Solution<Point3D>> solutions) {
956 solvePreliminarySolutions(samplesIndices, solutions);
957 }
958
959 @Override
960 public double computeResidual(final Solution<Point3D> currentEstimation, final int i) {
961 return residual(currentEstimation, i);
962 }
963
964 @Override
965 public boolean isReady() {
966 return PROMedSRobustRangingAndRssiRadioSourceEstimator3D.this.isReady();
967 }
968
969 @Override
970 public void onEstimateStart(final RobustEstimator<Solution<Point3D>> estimator) {
971 // no action needed
972 }
973
974 @Override
975 public void onEstimateEnd(final RobustEstimator<Solution<Point3D>> estimator) {
976 // no action needed
977 }
978
979 @Override
980 public void onEstimateNextIteration(
981 final RobustEstimator<Solution<Point3D>> estimator, final int iteration) {
982 if (listener != null) {
983 listener.onEstimateNextIteration(
984 PROMedSRobustRangingAndRssiRadioSourceEstimator3D.this, iteration);
985 }
986 }
987
988 @Override
989 public void onEstimateProgressChange(
990 final RobustEstimator<Solution<Point3D>> estimator, final float progress) {
991 if (listener != null) {
992 listener.onEstimateProgressChange(
993 PROMedSRobustRangingAndRssiRadioSourceEstimator3D.this, progress);
994 }
995 }
996 });
997
998 try {
999 locked = true;
1000
1001 if (listener != null) {
1002 listener.onEstimateStart(this);
1003 }
1004
1005 inliersData = null;
1006
1007 // inlier thresholds are disabled to obtain a less restrictive amount of inliers
1008 innerEstimator.setUseInlierThresholds(false);
1009
1010 innerEstimator.setConfidence(confidence);
1011 innerEstimator.setMaxIterations(maxIterations);
1012 innerEstimator.setProgressDelta(progressDelta);
1013 final var result = innerEstimator.estimate();
1014 inliersData = innerEstimator.getInliersData();
1015 attemptRefine(result);
1016
1017 if (listener != null) {
1018 listener.onEstimateEnd(this);
1019 }
1020
1021 } catch (final com.irurueta.numerical.LockedException e) {
1022 throw new LockedException(e);
1023 } catch (final com.irurueta.numerical.NotReadyException e) {
1024 throw new NotReadyException(e);
1025 } finally {
1026 locked = false;
1027 }
1028 }
1029
1030 /**
1031 * Returns method being used for robust estimation.
1032 *
1033 * @return method being used for robust estimation.
1034 */
1035 @Override
1036 public RobustEstimatorMethod getMethod() {
1037 return RobustEstimatorMethod.PROMEDS;
1038 }
1039
1040 /**
1041 * Sets quality scores corresponding to each provided sample.
1042 * This method is used internally and does not check whether instance is
1043 * locked or not.
1044 *
1045 * @param qualityScores quality scores to be set.
1046 * @throws IllegalArgumentException if provided quality scores length
1047 * is smaller than 3 samples.
1048 */
1049 private void internalSetQualityScores(final double[] qualityScores) {
1050 if (qualityScores == null || qualityScores.length < getMinReadings()) {
1051 throw new IllegalArgumentException();
1052 }
1053
1054 this.qualityScores = qualityScores;
1055 }
1056 }