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.LMedSRobustEstimator;
24 import com.irurueta.numerical.robust.LMedSRobustEstimatorListener;
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 LMedS 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 LMedSRobustRangingAndRssiRadioSourceEstimator2D<S extends RadioSource> extends
56 RobustRangingAndRssiRadioSourceEstimator2D<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 * Constructor.
102 */
103 public LMedSRobustRangingAndRssiRadioSourceEstimator2D() {
104 super();
105 }
106
107 /**
108 * Constructor.
109 * Sets signal readings belonging to the same radio source.
110 *
111 * @param readings signal readings belonging to the same radio source.
112 * @throws IllegalArgumentException if readings are not valid.
113 */
114 public LMedSRobustRangingAndRssiRadioSourceEstimator2D(
115 final List<? extends RangingAndRssiReadingLocated<S, Point2D>> readings) {
116 super(readings);
117 }
118
119 /**
120 * Constructor.
121 *
122 * @param listener listener in charge of attending events raised by this instance.
123 */
124 public LMedSRobustRangingAndRssiRadioSourceEstimator2D(
125 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point2D> listener) {
126 super(listener);
127 }
128
129 /**
130 * Constructor.
131 * Sets signal readings belonging to the same radio source.
132 *
133 * @param readings signal readings belonging to the same radio source.
134 * @param listener listener in charge of attending events raised by this instance.
135 * @throws IllegalArgumentException if readings are not valid.
136 */
137 public LMedSRobustRangingAndRssiRadioSourceEstimator2D(
138 final List<? extends RangingAndRssiReadingLocated<S, Point2D>> readings,
139 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point2D> listener) {
140 super(readings, listener);
141 }
142
143 /**
144 * Constructor.
145 * Sets signal readings belonging to the same radio source.
146 *
147 * @param readings signal readings belonging to the same radio source.
148 * @param initialPosition initial position to start the estimation of radio
149 * source position.
150 * @throws IllegalArgumentException if readings are not valid.
151 */
152 public LMedSRobustRangingAndRssiRadioSourceEstimator2D(
153 final List<? extends RangingAndRssiReadingLocated<S, Point2D>> readings, final Point2D initialPosition) {
154 super(readings, initialPosition);
155 }
156
157 /**
158 * Constructor.
159 *
160 * @param initialPosition initial position to start the estimation of radio
161 * source position.
162 */
163 public LMedSRobustRangingAndRssiRadioSourceEstimator2D(final Point2D initialPosition) {
164 super(initialPosition);
165 }
166
167 /**
168 * Constructor.
169 *
170 * @param initialPosition initial position to start the estimation of radio
171 * source position.
172 * @param listener listener in charge of attending events raised by this instance.
173 */
174 public LMedSRobustRangingAndRssiRadioSourceEstimator2D(
175 final Point2D initialPosition,
176 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point2D> listener) {
177 super(initialPosition, listener);
178 }
179
180 /**
181 * Constructor.
182 * Sets signal readings belonging to the same radio source.
183 *
184 * @param readings signal readings belonging to the same radio source.
185 * @param initialPosition initial position to start the estimation of radio
186 * source position.
187 * @param listener listener in charge of attending events raised by this instance.
188 * @throws IllegalArgumentException if readings are not valid.
189 */
190 public LMedSRobustRangingAndRssiRadioSourceEstimator2D(
191 final List<? extends RangingAndRssiReadingLocated<S, Point2D>> readings, final Point2D initialPosition,
192 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point2D> listener) {
193 super(readings, initialPosition, listener);
194 }
195
196 /**
197 * Constructor.
198 *
199 * @param initialTransmittedPowerdBm initial transmitted power to start the
200 * estimation of radio source transmitted power
201 * (expressed in dBm's)
202 */
203 public LMedSRobustRangingAndRssiRadioSourceEstimator2D(final Double initialTransmittedPowerdBm) {
204 super(initialTransmittedPowerdBm);
205 }
206
207 /**
208 * Constructor.
209 * Sets signal readings belonging to the same radio source.
210 *
211 * @param readings signal readings belonging to the same radio source.
212 * @param initialTransmittedPowerdBm initial transmitted power to start the
213 * estimation of radio source transmitted power
214 * (expressed in dBm's)
215 * @throws IllegalArgumentException if readings are not valid.
216 */
217 public LMedSRobustRangingAndRssiRadioSourceEstimator2D(
218 final List<? extends RangingAndRssiReadingLocated<S, Point2D>> readings,
219 final Double initialTransmittedPowerdBm) {
220 super(readings, initialTransmittedPowerdBm);
221 }
222
223 /**
224 * Constructor.
225 *
226 * @param initialTransmittedPowerdBm initial transmitted power to start the
227 * estimation of radio source transmitted power
228 * (expressed in dBm's)
229 * @param listener listener in charge of attending events raised by this instance.
230 */
231 public LMedSRobustRangingAndRssiRadioSourceEstimator2D(
232 final Double initialTransmittedPowerdBm,
233 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point2D> listener) {
234 super(initialTransmittedPowerdBm, listener);
235 }
236
237 /**
238 * Constructor.
239 * Sets signal readings belonging to the same radio source.
240 *
241 * @param readings signal readings belonging to the same radio source.
242 * @param initialTransmittedPowerdBm initial transmitted power to start the
243 * estimation of radio source transmitted power
244 * (expressed in dBm's)
245 * @param listener listener in charge of attending events raised by this instance.
246 * @throws IllegalArgumentException if readings are not valid.
247 */
248 public LMedSRobustRangingAndRssiRadioSourceEstimator2D(
249 final List<? extends RangingAndRssiReadingLocated<S, Point2D>> readings,
250 final Double initialTransmittedPowerdBm,
251 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point2D> listener) {
252 super(readings, initialTransmittedPowerdBm, listener);
253 }
254
255 /**
256 * Constructor.
257 * Sets signal readings belonging to the same radio source.
258 *
259 * @param readings signal readings belonging to the same radio source.
260 * @param initialPosition initial position to start the estimation of radio
261 * source position.
262 * @param initialTransmittedPowerdBm initial transmitted power to start the
263 * estimation of radio source transmitted power
264 * (expressed in dBm's).
265 * @throws IllegalArgumentException if readings are not valid.
266 */
267 public LMedSRobustRangingAndRssiRadioSourceEstimator2D(
268 final List<? extends RangingAndRssiReadingLocated<S, Point2D>> readings, final Point2D initialPosition,
269 final Double initialTransmittedPowerdBm) {
270 super(readings, initialPosition, initialTransmittedPowerdBm);
271 }
272
273 /**
274 * Constructor.
275 *
276 * @param initialPosition initial position to start the estimation of radio
277 * source position.
278 * @param initialTransmittedPowerdBm initial transmitted power to start the
279 * estimation of radio source transmitted power
280 * (expressed in dBm's).
281 */
282 public LMedSRobustRangingAndRssiRadioSourceEstimator2D(
283 final Point2D initialPosition, final Double initialTransmittedPowerdBm) {
284 super(initialPosition, initialTransmittedPowerdBm);
285 }
286
287 /**
288 * Constructor.
289 *
290 * @param initialPosition initial position to start the estimation of radio
291 * source position.
292 * @param initialTransmittedPowerdBm initial transmitted power to start the
293 * estimation of radio source transmitted power
294 * (expressed in dBm's).
295 * @param listener in charge of attending events raised by this instance.
296 */
297 public LMedSRobustRangingAndRssiRadioSourceEstimator2D(
298 final Point2D initialPosition, final Double initialTransmittedPowerdBm,
299 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point2D> listener) {
300 super(initialPosition, initialTransmittedPowerdBm, listener);
301 }
302
303 /**
304 * Constructor.
305 * Sets signal readings belonging to the same radio source.
306 *
307 * @param readings signal readings belonging to the same radio source.
308 * @param initialPosition initial position to start the estimation of radio
309 * source position.
310 * @param initialTransmittedPowerdBm initial transmitted power to start the
311 * estimation of radio source transmitted power
312 * (expressed in dBm's).
313 * @param listener listener in charge of attending events raised by this instance.
314 * @throws IllegalArgumentException if readings are not valid.
315 */
316 public LMedSRobustRangingAndRssiRadioSourceEstimator2D(
317 final List<? extends RangingAndRssiReadingLocated<S, Point2D>> readings,
318 final Point2D initialPosition, final Double initialTransmittedPowerdBm,
319 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point2D> listener) {
320 super(readings, initialPosition, initialTransmittedPowerdBm, listener);
321 }
322
323 /**
324 * Constructor.
325 * Sets signal readings belonging to the same radio source.
326 *
327 * @param readings signal readings belonging to the same radio source.
328 * @param initialPosition initial position to start the estimation of radio
329 * source position.
330 * @param initialTransmittedPowerdBm initial transmitted power to start the
331 * estimation of radio source transmitted power
332 * (expressed in dBm's).
333 * @param initialPathLossExponent initial path loss exponent. A typical value is 2.0.
334 * @throws IllegalArgumentException if readings are not valid.
335 */
336 public LMedSRobustRangingAndRssiRadioSourceEstimator2D(
337 final List<? extends RangingAndRssiReadingLocated<S, Point2D>> readings, final Point2D initialPosition,
338 final Double initialTransmittedPowerdBm, final double initialPathLossExponent) {
339 super(readings, initialPosition, initialTransmittedPowerdBm, initialPathLossExponent);
340 }
341
342 /**
343 * Constructor.
344 *
345 * @param initialPosition initial position to start the estimation of radio
346 * source position.
347 * @param initialTransmittedPowerdBm initial transmitted power to start the
348 * estimation of radio source transmitted power
349 * (expressed in dBm's).
350 * @param initialPathLossExponent initial path loss exponent. A typical value is 2.0.
351 */
352 public LMedSRobustRangingAndRssiRadioSourceEstimator2D(
353 final Point2D initialPosition, final Double initialTransmittedPowerdBm, final double initialPathLossExponent) {
354 super(initialPosition, initialTransmittedPowerdBm, initialPathLossExponent);
355 }
356
357 /**
358 * Constructor.
359 *
360 * @param initialPosition initial position to start the estimation of radio
361 * source position.
362 * @param initialTransmittedPowerdBm initial transmitted power to start the
363 * estimation of radio source transmitted power
364 * (expressed in dBm's).
365 * @param initialPathLossExponent initial path loss exponent. A typical value is 2.0.
366 * @param listener listener in charge of attending events raised by this instance.
367 */
368 public LMedSRobustRangingAndRssiRadioSourceEstimator2D(
369 final Point2D initialPosition, final Double initialTransmittedPowerdBm,
370 final double initialPathLossExponent,
371 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point2D> listener) {
372 super(initialPosition, initialTransmittedPowerdBm, initialPathLossExponent, listener);
373 }
374
375 /**
376 * Constructor.
377 * Sets signal readings belonging to the same radio source.
378 *
379 * @param readings signal readings belonging to the same radio source.
380 * @param initialPosition initial position to start the estimation of radio
381 * source position.
382 * @param initialTransmittedPowerdBm initial transmitted power to start the
383 * estimation of radio source transmitted power
384 * (expressed in dBm's).
385 * @param initialPathLossExponent initial path loss exponent. A typical value is 2.0.
386 * @param listener listener in charge of attending events raised by this instance.
387 * @throws IllegalArgumentException if readings are not valid.
388 */
389 public LMedSRobustRangingAndRssiRadioSourceEstimator2D(
390 final List<? extends RangingAndRssiReadingLocated<S, Point2D>> readings, final Point2D initialPosition,
391 final Double initialTransmittedPowerdBm, final double initialPathLossExponent,
392 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point2D> listener) {
393 super(readings, initialPosition, initialTransmittedPowerdBm, initialPathLossExponent, listener);
394 }
395
396 /**
397 * Returns threshold to be used to keep the algorithm iterating in case that
398 * best estimated threshold using median of residuals is not small enough.
399 * Once a solution is found that generates a threshold below this value, the
400 * algorithm will stop.
401 * The stop threshold can be used to prevent the LMedS algorithm to iterate
402 * too many times in cases where samples have a very similar accuracy.
403 * For instance, in cases where proportion of outliers is very small (close
404 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
405 * iterate for a long time trying to find the best solution when indeed
406 * there is no need to do that if a reasonable threshold has already been
407 * reached.
408 * Because of this behaviour the stop threshold can be set to a value much
409 * lower than the one typically used in RANSAC, and yet the algorithm could
410 * still produce even smaller thresholds in estimated results.
411 *
412 * @return stop threshold to stop the algorithm prematurely when a certain
413 * accuracy has been reached.
414 */
415 public double getStopThreshold() {
416 return stopThreshold;
417 }
418
419 /**
420 * Sets threshold to be used to keep the algorithm iterating in case that
421 * best estimated threshold using median of residuals is not small enough.
422 * Once a solution is found that generates a threshold below this value,
423 * the algorithm will stop.
424 * The stop threshold can be used to prevent the LMedS algorithm to iterate
425 * too many times in cases where samples have a very similar accuracy.
426 * For instance, in cases where proportion of outliers is very small (close
427 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
428 * iterate for a long time trying to find the best solution when indeed
429 * there is no need to do that if a reasonable threshold has already been
430 * reached.
431 * Because of this behaviour the stop threshold can be set to a value much
432 * lower than the one typically used in RANSAC, and yet the algorithm could
433 * still produce even smaller thresholds in estimated results.
434 *
435 * @param stopThreshold stop threshold to stop the algorithm prematurely
436 * when a certain accuracy has been reached.
437 * @throws IllegalArgumentException if provided value is zero or negative.
438 * @throws LockedException if this solver is locked.
439 */
440 public void setStopThreshold(final double stopThreshold) throws LockedException {
441 if (isLocked()) {
442 throw new LockedException();
443 }
444 if (stopThreshold <= MIN_STOP_THRESHOLD) {
445 throw new IllegalArgumentException();
446 }
447
448 this.stopThreshold = stopThreshold;
449 }
450
451 /**
452 * Robustly estimates position, transmitted power and path-loss exponent for a
453 * radio source.
454 *
455 * @throws LockedException if instance is busy during estimation.
456 * @throws NotReadyException if estimator is not ready.
457 * @throws RobustEstimatorException if estimation fails for any reason
458 * (i.e. numerical instability, no solution available, etc).
459 */
460 @Override
461 public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
462 if (isLocked()) {
463 throw new LockedException();
464 }
465 if (!isReady()) {
466 throw new NotReadyException();
467 }
468
469 final var innerEstimator = new LMedSRobustEstimator<>(
470 new LMedSRobustEstimatorListener<RobustRangingAndRssiRadioSourceEstimator.Solution<Point2D>>() {
471
472 @Override
473 public int getTotalSamples() {
474 return readings.size();
475 }
476
477 @Override
478 public int getSubsetSize() {
479 return Math.max(preliminarySubsetSize, getMinReadings());
480 }
481
482 @Override
483 public void estimatePreliminarSolutions(
484 final int[] samplesIndices,
485 final List<RobustRangingAndRssiRadioSourceEstimator.Solution<Point2D>> solutions) {
486 solvePreliminarySolutions(samplesIndices, solutions);
487 }
488
489 @Override
490 public double computeResidual(
491 final RobustRangingAndRssiRadioSourceEstimator.Solution<Point2D> currentEstimation,
492 final int i) {
493 return residual(currentEstimation, i);
494 }
495
496 @Override
497 public boolean isReady() {
498 return LMedSRobustRangingAndRssiRadioSourceEstimator2D.this.isReady();
499 }
500
501 @Override
502 public void onEstimateStart(
503 final RobustEstimator<RobustRangingAndRssiRadioSourceEstimator
504 .Solution<Point2D>> estimator) {
505 // no action needed
506 }
507
508 @Override
509 public void onEstimateEnd(
510 final RobustEstimator<RobustRangingAndRssiRadioSourceEstimator
511 .Solution<Point2D>> estimator) {
512 // no action needed
513 }
514
515 @Override
516 public void onEstimateNextIteration(
517 final RobustEstimator<RobustRangingAndRssiRadioSourceEstimator
518 .Solution<Point2D>> estimator,
519 final int iteration) {
520 if (listener != null) {
521 listener.onEstimateNextIteration(
522 LMedSRobustRangingAndRssiRadioSourceEstimator2D.this, iteration);
523 }
524 }
525
526 @Override
527 public void onEstimateProgressChange(
528 final RobustEstimator<RobustRangingAndRssiRadioSourceEstimator
529 .Solution<Point2D>> estimator,
530 final float progress) {
531 if (listener != null) {
532 listener.onEstimateProgressChange(
533 LMedSRobustRangingAndRssiRadioSourceEstimator2D.this, progress);
534 }
535 }
536 });
537
538 try {
539 locked = true;
540
541 if (listener != null) {
542 listener.onEstimateStart(this);
543 }
544
545 inliersData = null;
546 innerEstimator.setConfidence(confidence);
547 innerEstimator.setMaxIterations(maxIterations);
548 innerEstimator.setProgressDelta(progressDelta);
549 final var result = innerEstimator.estimate();
550 inliersData = innerEstimator.getInliersData();
551 attemptRefine(result);
552
553 if (listener != null) {
554 listener.onEstimateEnd(this);
555 }
556
557 } catch (final com.irurueta.numerical.LockedException e) {
558 throw new LockedException(e);
559 } catch (final com.irurueta.numerical.NotReadyException e) {
560 throw new NotReadyException(e);
561 } finally {
562 locked = false;
563 }
564 }
565
566 /**
567 * Returns method being used for robust estimation.
568 *
569 * @return method being used for robust estimation.
570 */
571 @Override
572 public RobustEstimatorMethod getMethod() {
573 return RobustEstimatorMethod.LMEDS;
574 }
575 }