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.Point2D;
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.RANSACRobustEstimator;
24 import com.irurueta.numerical.robust.RANSACRobustEstimatorListener;
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 2D 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 RANSACRobustRangingAndRssiRadioSourceEstimator2D<S extends RadioSource> extends
56 RobustRangingAndRssiRadioSourceEstimator2D<S> {
57
58 /**
59 * Constant defining default threshold on received power (RSSI) expressed in
60 * dBm's.
61 */
62 public static final double DEFAULT_THRESHOLD = 0.1;
63
64 /**
65 * Minimum value that can be set as threshold.
66 * Threshold must be strictly greater than 0.0.s
67 */
68 public static final double MIN_THRESHOLD = 0.0;
69
70 /**
71 * Indicates that by default inliers will only be computed but not kept.
72 */
73 public static final boolean DEFAULT_COMPUTE_AND_KEEP_INLIERS = false;
74
75 /**
76 * Indicates that by default residuals will only be computed but not kept.
77 */
78 public static final boolean DEFAULT_COMPUTE_AND_KEEP_RESIDUALS = false;
79
80 /**
81 * Threshold to determine whether samples are inliers or not when testing possible solutions.
82 * The threshold refers to the amount of error on received power (RSSI) expressed
83 * in dBm's between received value that should have been received on estimated
84 * iso-tropical model and actual measured value.
85 */
86 private double threshold = DEFAULT_THRESHOLD;
87
88 /**
89 * Indicates whether inliers must be computed and kept.
90 */
91 private boolean computeAndKeepInliers = DEFAULT_COMPUTE_AND_KEEP_INLIERS;
92
93 /**
94 * Indicates whether residuals must be computed and kept.
95 */
96 private boolean computeAndKeepResiduals = DEFAULT_COMPUTE_AND_KEEP_RESIDUALS;
97
98 /**
99 * Constructor.
100 */
101 public RANSACRobustRangingAndRssiRadioSourceEstimator2D() {
102 super();
103 }
104
105 /**
106 * Constructor.
107 * Sets signal readings belonging to the same radio source.
108 *
109 * @param readings signal readings belonging to the same radio source.
110 * @throws IllegalArgumentException if readings are not valid.
111 */
112 public RANSACRobustRangingAndRssiRadioSourceEstimator2D(
113 final List<? extends RangingAndRssiReadingLocated<S, Point2D>> readings) {
114 super(readings);
115 }
116
117 /**
118 * Constructor.
119 *
120 * @param listener listener in charge of attending events raised by this instance.
121 */
122 public RANSACRobustRangingAndRssiRadioSourceEstimator2D(
123 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point2D> listener) {
124 super(listener);
125 }
126
127 /**
128 * Constructor.
129 * Sets signal readings belonging to the same radio source.
130 *
131 * @param readings signal readings belonging to the same radio source.
132 * @param listener listener in charge of attending events raised by this instance.
133 * @throws IllegalArgumentException if readings are not valid.
134 */
135 public RANSACRobustRangingAndRssiRadioSourceEstimator2D(
136 final List<? extends RangingAndRssiReadingLocated<S, Point2D>> readings,
137 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point2D> listener) {
138 super(readings, listener);
139 }
140
141 /**
142 * Constructor.
143 * Sets signal readings belonging to the same radio source.
144 *
145 * @param readings signal readings belonging to the same radio source.
146 * @param initialPosition initial position to start the estimation of radio
147 * source position.
148 * @throws IllegalArgumentException if readings are not valid.
149 */
150 public RANSACRobustRangingAndRssiRadioSourceEstimator2D(
151 final List<? extends RangingAndRssiReadingLocated<S, Point2D>> readings, final Point2D initialPosition) {
152 super(readings, initialPosition);
153 }
154
155 /**
156 * Constructor.
157 *
158 * @param initialPosition initial position to start the estimation of radio
159 * source position.
160 */
161 public RANSACRobustRangingAndRssiRadioSourceEstimator2D(final Point2D initialPosition) {
162 super(initialPosition);
163 }
164
165 /**
166 * Constructor.
167 *
168 * @param initialPosition initial position to start the estimation of radio
169 * source position.
170 * @param listener listener in charge of attending events raised by this instance.
171 */
172 public RANSACRobustRangingAndRssiRadioSourceEstimator2D(
173 final Point2D initialPosition,
174 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point2D> listener) {
175 super(initialPosition, listener);
176 }
177
178 /**
179 * Constructor.
180 * Sets signal readings belonging to the same radio source.
181 *
182 * @param readings signal readings belonging to the same radio source.
183 * @param initialPosition initial position to start the estimation of radio
184 * source position.
185 * @param listener listener in charge of attending events raised by this instance.
186 * @throws IllegalArgumentException if readings are not valid.
187 */
188 public RANSACRobustRangingAndRssiRadioSourceEstimator2D(
189 final List<? extends RangingAndRssiReadingLocated<S, Point2D>> readings, final Point2D initialPosition,
190 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point2D> listener) {
191 super(readings, initialPosition, listener);
192 }
193
194 /**
195 * Constructor.
196 *
197 * @param initialTransmittedPowerdBm initial transmitted power to start the
198 * estimation of radio source transmitted power
199 * (expressed in dBm's)
200 */
201 public RANSACRobustRangingAndRssiRadioSourceEstimator2D(final Double initialTransmittedPowerdBm) {
202 super(initialTransmittedPowerdBm);
203 }
204
205 /**
206 * Constructor.
207 * Sets signal readings belonging to the same radio source.
208 *
209 * @param readings signal readings belonging to the same radio source.
210 * @param initialTransmittedPowerdBm initial transmitted power to start the
211 * estimation of radio source transmitted power
212 * (expressed in dBm's)
213 * @throws IllegalArgumentException if readings are not valid.
214 */
215 public RANSACRobustRangingAndRssiRadioSourceEstimator2D(
216 final List<? extends RangingAndRssiReadingLocated<S, Point2D>> readings,
217 final Double initialTransmittedPowerdBm) {
218 super(readings, initialTransmittedPowerdBm);
219 }
220
221 /**
222 * Constructor.
223 *
224 * @param initialTransmittedPowerdBm initial transmitted power to start the
225 * estimation of radio source transmitted power
226 * (expressed in dBm's)
227 * @param listener listener in charge of attending events raised by this instance.
228 */
229 public RANSACRobustRangingAndRssiRadioSourceEstimator2D(
230 final Double initialTransmittedPowerdBm,
231 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point2D> listener) {
232 super(initialTransmittedPowerdBm, listener);
233 }
234
235 /**
236 * Constructor.
237 * Sets signal readings belonging to the same radio source.
238 *
239 * @param readings signal readings belonging to the same radio source.
240 * @param initialTransmittedPowerdBm initial transmitted power to start the
241 * estimation of radio source transmitted power
242 * (expressed in dBm's)
243 * @param listener listener in charge of attending events raised by this instance.
244 * @throws IllegalArgumentException if readings are not valid.
245 */
246 public RANSACRobustRangingAndRssiRadioSourceEstimator2D(
247 final List<? extends RangingAndRssiReadingLocated<S, Point2D>> readings,
248 final Double initialTransmittedPowerdBm,
249 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point2D> listener) {
250 super(readings, initialTransmittedPowerdBm, listener);
251 }
252
253 /**
254 * Constructor.
255 * Sets signal readings belonging to the same radio source.
256 *
257 * @param readings signal readings belonging to the same radio source.
258 * @param initialPosition initial position to start the estimation of radio
259 * source position.
260 * @param initialTransmittedPowerdBm initial transmitted power to start the
261 * estimation of radio source transmitted power
262 * (expressed in dBm's).
263 * @throws IllegalArgumentException if readings are not valid.
264 */
265 public RANSACRobustRangingAndRssiRadioSourceEstimator2D(
266 final List<? extends RangingAndRssiReadingLocated<S, Point2D>> readings, final Point2D initialPosition,
267 final Double initialTransmittedPowerdBm) {
268 super(readings, initialPosition, initialTransmittedPowerdBm);
269 }
270
271 /**
272 * Constructor.
273 *
274 * @param initialPosition initial position to start the estimation of radio
275 * source position.
276 * @param initialTransmittedPowerdBm initial transmitted power to start the
277 * estimation of radio source transmitted power
278 * (expressed in dBm's).
279 */
280 public RANSACRobustRangingAndRssiRadioSourceEstimator2D(
281 final Point2D initialPosition, final Double initialTransmittedPowerdBm) {
282 super(initialPosition, initialTransmittedPowerdBm);
283 }
284
285 /**
286 * Constructor.
287 *
288 * @param initialPosition initial position to start the estimation of radio
289 * source position.
290 * @param initialTransmittedPowerdBm initial transmitted power to start the
291 * estimation of radio source transmitted power
292 * (expressed in dBm's).
293 * @param listener in charge of attending events raised by this instance.
294 */
295 public RANSACRobustRangingAndRssiRadioSourceEstimator2D(
296 final Point2D initialPosition, final Double initialTransmittedPowerdBm,
297 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point2D> listener) {
298 super(initialPosition, initialTransmittedPowerdBm, listener);
299 }
300
301 /**
302 * Constructor.
303 * Sets signal readings belonging to the same radio source.
304 *
305 * @param readings signal readings belonging to the same radio source.
306 * @param initialPosition initial position to start the estimation of radio
307 * source position.
308 * @param initialTransmittedPowerdBm initial transmitted power to start the
309 * estimation of radio source transmitted power
310 * (expressed in dBm's).
311 * @param listener listener in charge of attending events raised by this instance.
312 * @throws IllegalArgumentException if readings are not valid.
313 */
314 public RANSACRobustRangingAndRssiRadioSourceEstimator2D(
315 final List<? extends RangingAndRssiReadingLocated<S, Point2D>> readings, final Point2D initialPosition,
316 final Double initialTransmittedPowerdBm,
317 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point2D> listener) {
318 super(readings, initialPosition, initialTransmittedPowerdBm, listener);
319 }
320
321 /**
322 * Constructor.
323 * Sets signal readings belonging to the same radio source.
324 *
325 * @param readings signal readings belonging to the same radio source.
326 * @param initialPosition initial position to start the estimation of radio
327 * source position.
328 * @param initialTransmittedPowerdBm initial transmitted power to start the
329 * estimation of radio source transmitted power
330 * (expressed in dBm's).
331 * @param initialPathLossExponent initial path loss exponent. A typical value is 2.0.
332 * @throws IllegalArgumentException if readings are not valid.
333 */
334 public RANSACRobustRangingAndRssiRadioSourceEstimator2D(
335 final List<? extends RangingAndRssiReadingLocated<S, Point2D>> readings, final Point2D initialPosition,
336 final Double initialTransmittedPowerdBm, final double initialPathLossExponent) {
337 super(readings, initialPosition, initialTransmittedPowerdBm, initialPathLossExponent);
338 }
339
340 /**
341 * Constructor.
342 *
343 * @param initialPosition initial position to start the estimation of radio
344 * source position.
345 * @param initialTransmittedPowerdBm initial transmitted power to start the
346 * estimation of radio source transmitted power
347 * (expressed in dBm's).
348 * @param initialPathLossExponent initial path loss exponent. A typical value is 2.0.
349 */
350 public RANSACRobustRangingAndRssiRadioSourceEstimator2D(
351 final Point2D initialPosition, final Double initialTransmittedPowerdBm,
352 final double initialPathLossExponent) {
353 super(initialPosition, initialTransmittedPowerdBm, initialPathLossExponent);
354 }
355
356 /**
357 * Constructor.
358 *
359 * @param initialPosition initial position to start the estimation of radio
360 * source position.
361 * @param initialTransmittedPowerdBm initial transmitted power to start the
362 * estimation of radio source transmitted power
363 * (expressed in dBm's).
364 * @param initialPathLossExponent initial path loss exponent. A typical value is 2.0.
365 * @param listener listener in charge of attending events raised by this instance.
366 */
367 public RANSACRobustRangingAndRssiRadioSourceEstimator2D(
368 final Point2D initialPosition, final Double initialTransmittedPowerdBm,
369 final double initialPathLossExponent,
370 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point2D> listener) {
371 super(initialPosition, initialTransmittedPowerdBm, initialPathLossExponent, listener);
372 }
373
374 /**
375 * Constructor.
376 * Sets signal readings belonging to the same radio source.
377 *
378 * @param readings signal readings belonging to the same radio source.
379 * @param initialPosition initial position to start the estimation of radio
380 * source position.
381 * @param initialTransmittedPowerdBm initial transmitted power to start the
382 * estimation of radio source transmitted power
383 * (expressed in dBm's).
384 * @param initialPathLossExponent initial path loss exponent. A typical value is 2.0.
385 * @param listener listener in charge of attending events raised by this instance.
386 * @throws IllegalArgumentException if readings are not valid.
387 */
388 public RANSACRobustRangingAndRssiRadioSourceEstimator2D(
389 final List<? extends RangingAndRssiReadingLocated<S, Point2D>> readings, final Point2D initialPosition,
390 final Double initialTransmittedPowerdBm, final double initialPathLossExponent,
391 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point2D> listener) {
392 super(readings, initialPosition, initialTransmittedPowerdBm, initialPathLossExponent, listener);
393 }
394
395 /**
396 * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
397 * The threshold refers to the amount of error on received power (RSSI) expressed
398 * in dBm's between received value that should have been received on estimated
399 * iso-tropical model and actual measured value.
400 *
401 * @return threshold to determine whether samples are inliers or not.
402 */
403 public double getThreshold() {
404 return threshold;
405 }
406
407 /**
408 * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
409 * The threshold refers to the amount of error on received power (RSSI) expressed
410 * in dBm's between received value that should have been received on estimated
411 * iso-tropical model and actual measured value.
412 *
413 * @param threshold threshold to determine whether samples are inliers or not.
414 * @throws IllegalArgumentException if provided value is equal or less than zero.
415 * @throws LockedException if this estimator is locked.
416 */
417 public void setThreshold(final double threshold) throws LockedException {
418 if (isLocked()) {
419 throw new LockedException();
420 }
421 if (threshold <= MIN_THRESHOLD) {
422 throw new IllegalArgumentException();
423 }
424 this.threshold = threshold;
425 }
426
427
428 /**
429 * Indicates whether inliers must be computed and kept.
430 *
431 * @return true if inliers must be computed and kept, false if inliers
432 * only need to be computed but not kept.
433 */
434 public boolean isComputeAndKeepInliersEnabled() {
435 return computeAndKeepInliers;
436 }
437
438 /**
439 * Specifies whether inliers must be computed and kept.
440 *
441 * @param computeAndKeepInliers true if inliers must be computed and kept,
442 * false if inliers only need to be computed but not kept.
443 * @throws LockedException if this solver is locked.
444 */
445 public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
446 if (isLocked()) {
447 throw new LockedException();
448 }
449 this.computeAndKeepInliers = computeAndKeepInliers;
450 }
451
452 /**
453 * Indicates whether residuals must be computed and kept.
454 *
455 * @return true if residuals must be computed and kept, false if residuals
456 * only need to be computed but not kept.
457 */
458 public boolean isComputeAndKeepResidualsEnabled() {
459 return computeAndKeepResiduals;
460 }
461
462 /**
463 * Specifies whether residuals must be computed and kept.
464 *
465 * @param computeAndKeepResiduals true if residuals must be computed and kept,
466 * false if residuals only need to be computed but not kept.
467 * @throws LockedException if this solver is locked.
468 */
469 public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
470 if (isLocked()) {
471 throw new LockedException();
472 }
473 this.computeAndKeepResiduals = computeAndKeepResiduals;
474 }
475
476 /**
477 * Robustly estimates position, transmitted power and path-loss exponent for a
478 * radio source.
479 *
480 * @throws LockedException if instance is busy during estimation.
481 * @throws NotReadyException if estimator is not ready.
482 * @throws RobustEstimatorException if estimation fails for any reason
483 * (i.e. numerical instability, no solution available, etc).
484 */
485 @Override
486 public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
487 if (isLocked()) {
488 throw new LockedException();
489 }
490 if (!isReady()) {
491 throw new NotReadyException();
492 }
493
494 final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<Solution<Point2D>>() {
495 @Override
496 public double getThreshold() {
497 return threshold;
498 }
499
500 @Override
501 public int getTotalSamples() {
502 return readings.size();
503 }
504
505 @Override
506 public int getSubsetSize() {
507 return Math.max(preliminarySubsetSize, getMinReadings());
508 }
509
510 @Override
511 public void estimatePreliminarSolutions(
512 final int[] samplesIndices, final List<Solution<Point2D>> solutions) {
513 solvePreliminarySolutions(samplesIndices, solutions);
514 }
515
516 @Override
517 public double computeResidual(final Solution<Point2D> currentEstimation, final int i) {
518 return residual(currentEstimation, i);
519 }
520
521 @Override
522 public boolean isReady() {
523 return RANSACRobustRangingAndRssiRadioSourceEstimator2D.this.isReady();
524 }
525
526 @Override
527 public void onEstimateStart(final RobustEstimator<Solution<Point2D>> estimator) {
528 // no action needed
529 }
530
531 @Override
532 public void onEstimateEnd(final RobustEstimator<Solution<Point2D>> estimator) {
533 // no action needed
534 }
535
536 @Override
537 public void onEstimateNextIteration(
538 final RobustEstimator<Solution<Point2D>> estimator, final int iteration) {
539 if (listener != null) {
540 listener.onEstimateNextIteration(
541 RANSACRobustRangingAndRssiRadioSourceEstimator2D.this, iteration);
542 }
543 }
544
545 @Override
546 public void onEstimateProgressChange(
547 final RobustEstimator<Solution<Point2D>> estimator, final float progress) {
548 if (listener != null) {
549 listener.onEstimateProgressChange(
550 RANSACRobustRangingAndRssiRadioSourceEstimator2D.this, progress);
551 }
552 }
553 });
554
555 try {
556 locked = true;
557
558 if (listener != null) {
559 listener.onEstimateStart(this);
560 }
561
562 inliersData = null;
563 innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
564 innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
565 innerEstimator.setConfidence(confidence);
566 innerEstimator.setMaxIterations(maxIterations);
567 innerEstimator.setProgressDelta(progressDelta);
568 final var result = innerEstimator.estimate();
569 inliersData = innerEstimator.getInliersData();
570 attemptRefine(result);
571
572 if (listener != null) {
573 listener.onEstimateEnd(this);
574 }
575
576 } catch (final com.irurueta.numerical.LockedException e) {
577 throw new LockedException(e);
578 } catch (final com.irurueta.numerical.NotReadyException e) {
579 throw new NotReadyException(e);
580 } finally {
581 locked = false;
582 }
583 }
584
585 /**
586 * Returns method being used for robust estimation.
587 *
588 * @return method being used for robust estimation.
589 */
590 @Override
591 public RobustEstimatorMethod getMethod() {
592 return RobustEstimatorMethod.RANSAC;
593 }
594 }