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.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 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 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 LMedSRobustRangingAndRssiRadioSourceEstimator3D<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 * Constructor.
102 */
103 public LMedSRobustRangingAndRssiRadioSourceEstimator3D() {
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 LMedSRobustRangingAndRssiRadioSourceEstimator3D(
115 final List<? extends RangingAndRssiReadingLocated<S, Point3D>> 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 LMedSRobustRangingAndRssiRadioSourceEstimator3D(
125 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point3D> 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 LMedSRobustRangingAndRssiRadioSourceEstimator3D(
138 final List<? extends RangingAndRssiReadingLocated<S, Point3D>> readings,
139 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point3D> 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 LMedSRobustRangingAndRssiRadioSourceEstimator3D(
153 final List<? extends RangingAndRssiReadingLocated<S, Point3D>> readings, final Point3D 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 LMedSRobustRangingAndRssiRadioSourceEstimator3D(final Point3D 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 LMedSRobustRangingAndRssiRadioSourceEstimator3D(
175 final Point3D initialPosition,
176 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point3D> 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 LMedSRobustRangingAndRssiRadioSourceEstimator3D(
191 final List<? extends RangingAndRssiReadingLocated<S, Point3D>> readings, final Point3D initialPosition,
192 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point3D> 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 LMedSRobustRangingAndRssiRadioSourceEstimator3D(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 LMedSRobustRangingAndRssiRadioSourceEstimator3D(
218 final List<? extends RangingAndRssiReadingLocated<S, Point3D>> 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 LMedSRobustRangingAndRssiRadioSourceEstimator3D(
232 final Double initialTransmittedPowerdBm,
233 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point3D> 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 LMedSRobustRangingAndRssiRadioSourceEstimator3D(
249 final List<? extends RangingAndRssiReadingLocated<S, Point3D>> readings,
250 final Double initialTransmittedPowerdBm,
251 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point3D> 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 LMedSRobustRangingAndRssiRadioSourceEstimator3D(
268 final List<? extends RangingAndRssiReadingLocated<S, Point3D>> readings, final Point3D 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 LMedSRobustRangingAndRssiRadioSourceEstimator3D(
283 final Point3D 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 LMedSRobustRangingAndRssiRadioSourceEstimator3D(
298 final Point3D initialPosition, final Double initialTransmittedPowerdBm,
299 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point3D> 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 LMedSRobustRangingAndRssiRadioSourceEstimator3D(
317 final List<? extends RangingAndRssiReadingLocated<S, Point3D>> readings, final Point3D initialPosition,
318 final Double initialTransmittedPowerdBm,
319 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point3D> 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 LMedSRobustRangingAndRssiRadioSourceEstimator3D(
337 final List<? extends RangingAndRssiReadingLocated<S, Point3D>> readings, final Point3D 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 LMedSRobustRangingAndRssiRadioSourceEstimator3D(
353 final Point3D initialPosition, final Double initialTransmittedPowerdBm,
354 final double initialPathLossExponent) {
355 super(initialPosition, initialTransmittedPowerdBm, initialPathLossExponent);
356 }
357
358 /**
359 * Constructor.
360 *
361 * @param initialPosition initial position to start the estimation of radio
362 * source position.
363 * @param initialTransmittedPowerdBm initial transmitted power to start the
364 * estimation of radio source transmitted power
365 * (expressed in dBm's).
366 * @param initialPathLossExponent initial path loss exponent. A typical value is 2.0.
367 * @param listener listener in charge of attending events raised by this instance.
368 */
369 public LMedSRobustRangingAndRssiRadioSourceEstimator3D(
370 final Point3D initialPosition, final Double initialTransmittedPowerdBm,
371 final double initialPathLossExponent,
372 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point3D> listener) {
373 super(initialPosition, initialTransmittedPowerdBm, initialPathLossExponent, listener);
374 }
375
376 /**
377 * Constructor.
378 * Sets signal readings belonging to the same radio source.
379 *
380 * @param readings signal readings belonging to the same radio source.
381 * @param initialPosition initial position to start the estimation of radio
382 * source position.
383 * @param initialTransmittedPowerdBm initial transmitted power to start the
384 * estimation of radio source transmitted power
385 * (expressed in dBm's).
386 * @param initialPathLossExponent initial path loss exponent. A typical value is 2.0.
387 * @param listener listener in charge of attending events raised by this instance.
388 * @throws IllegalArgumentException if readings are not valid.
389 */
390 public LMedSRobustRangingAndRssiRadioSourceEstimator3D(
391 final List<? extends RangingAndRssiReadingLocated<S, Point3D>> readings, final Point3D initialPosition,
392 final Double initialTransmittedPowerdBm, final double initialPathLossExponent,
393 final RobustRangingAndRssiRadioSourceEstimatorListener<S, Point3D> listener) {
394 super(readings, initialPosition, initialTransmittedPowerdBm, initialPathLossExponent, listener);
395 }
396
397 /**
398 * Returns threshold to be used to keep the algorithm iterating in case that
399 * best estimated threshold using median of residuals is not small enough.
400 * Once a solution is found that generates a threshold below this value, the
401 * algorithm will stop.
402 * The stop threshold can be used to prevent the LMedS algorithm to iterate
403 * too many times in cases where samples have a very similar accuracy.
404 * For instance, in cases where proportion of outliers is very small (close
405 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
406 * iterate for a long time trying to find the best solution when indeed
407 * there is no need to do that if a reasonable threshold has already been
408 * reached.
409 * Because of this behaviour the stop threshold can be set to a value much
410 * lower than the one typically used in RANSAC, and yet the algorithm could
411 * still produce even smaller thresholds in estimated results.
412 *
413 * @return stop threshold to stop the algorithm prematurely when a certain
414 * accuracy has been reached.
415 */
416 public double getStopThreshold() {
417 return stopThreshold;
418 }
419
420 /**
421 * Sets threshold to be used to keep the algorithm iterating in case that
422 * best estimated threshold using median of residuals is not small enough.
423 * Once a solution is found that generates a threshold below this value,
424 * the algorithm will stop.
425 * The stop threshold can be used to prevent the LMedS algorithm to iterate
426 * too many times in cases where samples have a very similar accuracy.
427 * For instance, in cases where proportion of outliers is very small (close
428 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
429 * iterate for a long time trying to find the best solution when indeed
430 * there is no need to do that if a reasonable threshold has already been
431 * reached.
432 * Because of this behaviour the stop threshold can be set to a value much
433 * lower than the one typically used in RANSAC, and yet the algorithm could
434 * still produce even smaller thresholds in estimated results.
435 *
436 * @param stopThreshold stop threshold to stop the algorithm prematurely
437 * when a certain accuracy has been reached.
438 * @throws IllegalArgumentException if provided value is zero or negative.
439 * @throws LockedException if this solver is locked.
440 */
441 public void setStopThreshold(final double stopThreshold) throws LockedException {
442 if (isLocked()) {
443 throw new LockedException();
444 }
445 if (stopThreshold <= MIN_STOP_THRESHOLD) {
446 throw new IllegalArgumentException();
447 }
448
449 this.stopThreshold = stopThreshold;
450 }
451
452 /**
453 * Robustly estimates position, transmitted power and path-loss exponent for a
454 * radio source.
455 *
456 * @throws LockedException if instance is busy during estimation.
457 * @throws NotReadyException if estimator is not ready.
458 * @throws RobustEstimatorException if estimation fails for any reason
459 * (i.e. numerical instability, no solution available, etc).
460 */
461 @Override
462 public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
463 if (isLocked()) {
464 throw new LockedException();
465 }
466 if (!isReady()) {
467 throw new NotReadyException();
468 }
469
470 final var innerEstimator = new LMedSRobustEstimator<>(
471 new LMedSRobustEstimatorListener<RobustRangingAndRssiRadioSourceEstimator.Solution<Point3D>>() {
472
473 @Override
474 public int getTotalSamples() {
475 return readings.size();
476 }
477
478 @Override
479 public int getSubsetSize() {
480 return Math.max(preliminarySubsetSize, getMinReadings());
481 }
482
483 @Override
484 public void estimatePreliminarSolutions(
485 final int[] samplesIndices,
486 final List<RobustRangingAndRssiRadioSourceEstimator.Solution<Point3D>> solutions) {
487 solvePreliminarySolutions(samplesIndices, solutions);
488 }
489
490 @Override
491 public double computeResidual(
492 final RobustRangingAndRssiRadioSourceEstimator.Solution<Point3D> currentEstimation,
493 final int i) {
494 return residual(currentEstimation, i);
495 }
496
497 @Override
498 public boolean isReady() {
499 return LMedSRobustRangingAndRssiRadioSourceEstimator3D.this.isReady();
500 }
501
502 @Override
503 public void onEstimateStart(
504 final RobustEstimator<RobustRangingAndRssiRadioSourceEstimator
505 .Solution<Point3D>> estimator) {
506 // no action needed
507 }
508
509 @Override
510 public void onEstimateEnd(
511 final RobustEstimator<RobustRangingAndRssiRadioSourceEstimator
512 .Solution<Point3D>> estimator) {
513 // no action needed
514 }
515
516 @Override
517 public void onEstimateNextIteration(
518 final RobustEstimator<RobustRangingAndRssiRadioSourceEstimator
519 .Solution<Point3D>> estimator,
520 final int iteration) {
521 if (listener != null) {
522 listener.onEstimateNextIteration(
523 LMedSRobustRangingAndRssiRadioSourceEstimator3D.this, iteration);
524 }
525 }
526
527 @Override
528 public void onEstimateProgressChange(
529 final RobustEstimator<RobustRangingAndRssiRadioSourceEstimator
530 .Solution<Point3D>> estimator,
531 final float progress) {
532 if (listener != null) {
533 listener.onEstimateProgressChange(
534 LMedSRobustRangingAndRssiRadioSourceEstimator3D.this, progress);
535 }
536 }
537 });
538
539 try {
540 locked = true;
541
542 if (listener != null) {
543 listener.onEstimateStart(this);
544 }
545
546 inliersData = null;
547 innerEstimator.setConfidence(confidence);
548 innerEstimator.setMaxIterations(maxIterations);
549 innerEstimator.setProgressDelta(progressDelta);
550 final var result = innerEstimator.estimate();
551 inliersData = innerEstimator.getInliersData();
552 attemptRefine(result);
553
554 if (listener != null) {
555 listener.onEstimateEnd(this);
556 }
557
558 } catch (final com.irurueta.numerical.LockedException e) {
559 throw new LockedException(e);
560 } catch (final com.irurueta.numerical.NotReadyException e) {
561 throw new NotReadyException(e);
562 } finally {
563 locked = false;
564 }
565 }
566
567 /**
568 * Returns method being used for robust estimation.
569 *
570 * @return method being used for robust estimation.
571 */
572 @Override
573 public RobustEstimatorMethod getMethod() {
574 return RobustEstimatorMethod.LMEDS;
575 }
576 }