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