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.position;
17
18 import com.irurueta.algebra.AlgebraException;
19 import com.irurueta.algebra.Matrix;
20 import com.irurueta.algebra.SingularValueDecomposer;
21 import com.irurueta.geometry.Point;
22 import com.irurueta.navigation.indoor.*;
23 import com.irurueta.navigation.indoor.radiosource.RssiRadioSourceEstimator;
24 import com.irurueta.statistics.MultivariateNormalDist;
25
26 import java.util.List;
27
28 /**
29 * Utility class that converts located radio sources and fingerprints into positions,
30 * distances and distance standard deviations that can be used to solve the lateration
31 * problem.
32 */
33 public class PositionEstimatorHelper {
34
35 /**
36 * Constructor to prevent instantiation.
37 */
38 private PositionEstimatorHelper() {
39 }
40
41 /**
42 * Builds positions and distances from provided located radio sources and
43 * fingerprint readings.
44 * Notice that positions and distances lists might not have the same size
45 * as provided sources list or fingerprint readings list if not all radio sources
46 * between sources and fingerprint readings match.
47 * If no sources, fingerprint readings, positions and distances are provided, this
48 * method makes no action.
49 *
50 * @param sources located radio sources to obtain positions and other
51 * parameters.
52 * @param fingerprint fingerprint containing ranged RSSI readings.
53 * @param positions list where extracted positions will be stored.
54 * @param distances list where extracted distances will be stored.
55 * @param <P> a {@link Point} type.
56 */
57 public static <P extends Point<?>> void buildPositionsAndDistances(
58 final List<? extends RadioSourceLocated<P>> sources,
59 final Fingerprint<? extends RadioSource, ? extends Reading<? extends RadioSource>> fingerprint,
60 final List<P> positions, final List<Double> distances) {
61
62 if (sources == null || fingerprint == null || fingerprint.getReadings() == null || positions == null
63 || distances == null) {
64 return;
65 }
66
67 positions.clear();
68 distances.clear();
69
70 final var readings = fingerprint.getReadings();
71 for (final var reading : readings) {
72 //noinspection SuspiciousMethodCalls
73 final var index = sources.indexOf(reading.getSource());
74 if (index >= 0) {
75 final var locatedSource = sources.get(index);
76 final var position = locatedSource.getPosition();
77
78 // compute distance
79 Double distance1 = null;
80 Double distance2 = null;
81 switch (reading.getType()) {
82 case RANGING_READING:
83 distance1 = computeDistanceRanging((RangingReading<? extends RadioSource>) reading);
84 break;
85 case RSSI_READING:
86 distance1 = computeDistanceRssi(locatedSource, (RssiReading<? extends RadioSource>) reading);
87 break;
88 case RANGING_AND_RSSI_READING:
89 // in this case two positions and distance might be added to
90 // the lateration solver
91 distance1 = computeDistanceRanging((RangingAndRssiReading<? extends RadioSource>) reading);
92 distance2 = computeDistanceRssi(locatedSource,
93 (RangingAndRssiReading<? extends RadioSource>) reading);
94 break;
95 default:
96 break;
97 }
98
99 if (position != null) {
100 if (distance1 != null) {
101 positions.add(position);
102 distances.add(distance1);
103 }
104 if (distance2 != null) {
105 positions.add(position);
106 distances.add(distance2);
107 }
108 }
109 }
110 }
111 }
112
113 /**
114 * Builds positions, distances and standard deviations from provided located radio
115 * sources and fingerprint readings.
116 * Notice that positions, distances and standard deviations lists might not have
117 * the same size as provided sources list or fingerprint readings list if not all
118 * radio sources between sources and fingerprint readings match.
119 * If no sources, fingerprint readings, positions, distances and standard deviations
120 * are provided, this method makes no action.
121 *
122 * @param sources located radio sources to obtain positions
123 * and other parameters.
124 * @param fingerprint fingerprint containing ranged or RSSI
125 * readings.
126 * @param useRadioSourcePositionCovariance true to take into account radio source
127 * position covariance, false otherwise.
128 * @param fallbackDistanceStandardDeviation distance standard deviation to be
129 * assumed when it cannot be determined.
130 * @param positions list where extracted positions will be
131 * stored.
132 * @param distances list where extracted distances will be
133 * stored.
134 * @param distanceStandardDeviations list where extracted standard deviations
135 * of distances will be stored.
136 * @param <P> a {@link Point} type.
137 * @throws IllegalArgumentException if provided distance standard deviation fallback
138 * is negative.
139 */
140 public static <P extends Point<?>> void buildPositionsDistancesAndDistanceStandardDeviations(
141 final List<? extends RadioSourceLocated<P>> sources,
142 final Fingerprint<? extends RadioSource, ? extends Reading<? extends RadioSource>> fingerprint,
143 final boolean useRadioSourcePositionCovariance, final double fallbackDistanceStandardDeviation,
144 final List<P> positions, final List<Double> distances, final List<Double> distanceStandardDeviations) {
145 buildPositionsDistancesDistanceStandardDeviationsAndQualityScores(sources, fingerprint, null,
146 null, useRadioSourcePositionCovariance, fallbackDistanceStandardDeviation,
147 positions, distances, distanceStandardDeviations, null);
148 }
149
150 /**
151 * Builds positions, distances and standard deviations from provided located radio
152 * sources and fingerprint readings.
153 * Notice that positions, distance and standard deviations lists might not have the
154 * same size as provided sources list or fingerprint readings list if not all radio
155 * sources between sources and fingerprint readings match.
156 * If no sources, fingerprint readings, positions, distances and standard deviations
157 * are provided, this method makes no action.
158 *
159 * @param sources located radio sources to obtain
160 * positions and other parameters.
161 * @param fingerprint fingerprint containing ranged or RSSI
162 * readings.
163 * @param sourceQualityScores quality scores corresponding to each
164 * provided located radio source. The larger
165 * the score value the better the quality of
166 * the sample. If null, no quality scores
167 * will be stored.
168 * @param fingerprintReadingsQualityScores quality scores corresponding to each
169 * reading within provided fingerprint.
170 * @param useRadioSourcePositionCovariance true to take into account radio source
171 * position covariance, false otherwise.
172 * @param fallbackDistanceStandardDeviation distance standard deviation to be
173 * assumed when it cannot be determined.
174 * @param positions list where extracted positions will be stored.
175 * @param distances list where extracted distances will be stored.
176 * @param distanceStandardDeviations list where extracted standard deviations of
177 * distances will be stored.
178 * @param distanceQualityScores list where extracted quality scores will
179 * be stored. If null, quality scores will
180 * be ignored.
181 * @param <P> a {@link Point} type.
182 * @throws IllegalArgumentException if provided distance standard deviation
183 * fallback is negative.
184 */
185 @SuppressWarnings("DuplicatedCode")
186 public static <P extends Point<?>> void buildPositionsDistancesDistanceStandardDeviationsAndQualityScores(
187 final List<? extends RadioSourceLocated<P>> sources,
188 final Fingerprint<? extends RadioSource, ? extends Reading<? extends RadioSource>> fingerprint,
189 final double[] sourceQualityScores, final double[] fingerprintReadingsQualityScores,
190 final boolean useRadioSourcePositionCovariance, final double fallbackDistanceStandardDeviation,
191 final List<P> positions, final List<Double> distances, final List<Double> distanceStandardDeviations,
192 final List<Double> distanceQualityScores) {
193
194 if (fallbackDistanceStandardDeviation < 0.0) {
195 throw new IllegalArgumentException();
196 }
197
198 if (sources == null || fingerprint == null || fingerprint.getReadings() == null || positions == null
199 || distances == null || distanceStandardDeviations == null) {
200 return;
201 }
202
203 positions.clear();
204 distances.clear();
205 distanceStandardDeviations.clear();
206
207 if ((sourceQualityScores != null || fingerprintReadingsQualityScores != null)
208 && distanceQualityScores != null) {
209 distanceQualityScores.clear();
210 }
211
212 final var result1 = new Double[2];
213 final var result2 = new Double[2];
214
215 final var readings = fingerprint.getReadings();
216 var readingIndex = 0;
217 for (final var reading : readings) {
218 //noinspection SuspiciousMethodCalls
219 final var sourceIndex = sources.indexOf(reading.getSource());
220 final var readingQualityScore = fingerprintReadingsQualityScores != null
221 ? fingerprintReadingsQualityScores[readingIndex] : null;
222 Double sourceQualityScore = null;
223 Double qualityScore = null;
224 if (sourceIndex >= 0) {
225 final var locatedSource = sources.get(sourceIndex);
226 final var position = locatedSource.getPosition();
227 if (sourceQualityScores != null) {
228 sourceQualityScore = sourceQualityScores[sourceIndex];
229 }
230 readingIndex++;
231
232 if (readingQualityScore != null || sourceQualityScore != null) {
233 qualityScore = 0.0;
234 if (readingQualityScore != null) {
235 qualityScore += readingQualityScore;
236 }
237 if (sourceQualityScore != null) {
238 qualityScore += sourceQualityScore;
239 }
240 }
241
242 Matrix positionCovariance = null;
243 if (useRadioSourcePositionCovariance) {
244 positionCovariance = locatedSource.getPositionCovariance();
245 }
246
247 Double positionStandardDeviation = null;
248 if (positionCovariance != null) {
249 try {
250 // compute standard deviation associated to position
251 // uncertainty
252 final var decomposer = new SingularValueDecomposer(positionCovariance);
253 decomposer.decompose();
254
255 // singular values contain variances on each principal axis
256 final var singularValues = decomposer.getSingularValues();
257
258 // compute average of singular values as an "average" variance
259 // of position
260 var variance = 0.0;
261 for (final var singularValue : singularValues) {
262 variance += singularValue / singularValues.length;
263 }
264
265 positionStandardDeviation = Math.sqrt(variance);
266
267 } catch (final AlgebraException ignore) {
268 // no action needed
269 }
270 }
271
272 // compute distance and standard deviation
273 result1[0] = result1[1] = result2[0] = result2[1] = null;
274 switch (reading.getType()) {
275 case RANGING_READING:
276 computeDistanceAndStandardDeviationRanging(
277 (RangingReading<? extends RadioSource>) reading, positionStandardDeviation, result1);
278 break;
279 case RSSI_READING:
280 computeDistanceAndStandardDeviationRssi(locatedSource,
281 (RssiReading<? extends RadioSource>) reading, positionStandardDeviation, result1);
282 break;
283 case RANGING_AND_RSSI_READING:
284 computeDistanceAndStandardDeviationRanging(
285 (RangingAndRssiReading<? extends RadioSource>) reading, positionStandardDeviation,
286 result1);
287 computeDistanceAndStandardDeviationRssi(locatedSource,
288 (RangingAndRssiReading<? extends RadioSource>) reading, positionStandardDeviation,
289 result2);
290 break;
291 default:
292 break;
293 }
294
295 if (position != null) {
296 final var distance1 = result1[0];
297 final var distance2 = result2[0];
298 if (distance1 != null) {
299 final var standardDeviation1 = result1[1];
300
301 positions.add(position);
302 distances.add(distance1);
303 distanceStandardDeviations.add(standardDeviation1 != null ? standardDeviation1
304 : fallbackDistanceStandardDeviation);
305
306 if (qualityScore != null && distanceQualityScores != null) {
307 distanceQualityScores.add(qualityScore);
308 }
309 }
310
311 if (distance2 != null) {
312 final var standardDeviation2 = result2[1];
313
314 positions.add(position);
315 distances.add(distance2);
316 distanceStandardDeviations.add(standardDeviation2 != null ? standardDeviation2
317 : fallbackDistanceStandardDeviation);
318
319 if (qualityScore != null && distanceQualityScores != null) {
320 distanceQualityScores.add(qualityScore);
321 }
322 }
323 }
324 }
325 }
326 }
327
328 /**
329 * Obtains distance for a ranging reading.
330 *
331 * @param reading a ranging reading.
332 * @return distance to reading source or null if not available.
333 */
334 private static Double computeDistanceRanging(final RangingReading<? extends RadioSource> reading) {
335 return reading.getDistance();
336 }
337
338 /**
339 * Obtains distance for a ranging reading.
340 *
341 * @param reading a ranging reading.
342 * @return distance to reading source or null if not available.
343 */
344 private static Double computeDistanceRanging(final RangingAndRssiReading<? extends RadioSource> reading) {
345 return reading.getDistance();
346 }
347
348 /**
349 * Obtains distance for an RSSI reading.
350 *
351 * @param locatedSource a located source, that must also have power information.
352 * @param reading an RSSI reading.
353 * @param <P> a {@link Point} type.
354 * @return estimated distance or null if not available.
355 */
356 private static <P extends Point<?>> Double computeDistanceRssi(
357 final RadioSourceLocated<P> locatedSource, final RssiReading<? extends RadioSource> reading) {
358 return computeDistanceRssi(locatedSource, reading.getRssi());
359 }
360
361 /**
362 * Obtains distance for a ranging and RSSI reading.
363 *
364 * @param locatedSource a located source, that must also have power information.
365 * @param reading a ranging and RSSI reading.
366 * @param <P> a {@link Point} type.
367 * @return estimated distance or null if not available.
368 */
369 private static <P extends Point<?>> Double computeDistanceRssi(
370 final RadioSourceLocated<P> locatedSource, final RangingAndRssiReading<? extends RadioSource> reading) {
371 return computeDistanceRssi(locatedSource, reading.getRssi());
372 }
373
374 /**
375 * Obtains distance.
376 *
377 * @param locatedSource a located source, that must also have power information.
378 * @param rxPower ;
379 * @param <P> a {@link Point} type.
380 * @return estimated distance or null if not available.
381 */
382 private static <P extends Point<?>> Double computeDistanceRssi(
383 final RadioSourceLocated<P> locatedSource, final double rxPower) {
384 if (!(locatedSource instanceof RadioSourceWithPower poweredSource)) {
385 return null;
386 }
387
388 // source related parameters:
389
390 // transmitted power in dBm's
391 final var txPower = poweredSource.getTransmittedPower();
392
393 // path loss exponent
394 final var pathLossExponent = poweredSource.getPathLossExponent();
395
396 final var frequency = poweredSource.getFrequency();
397 final var k = RssiRadioSourceEstimator.SPEED_OF_LIGHT / (4.0 * Math.PI * frequency);
398 final var kdB = 10.0 * Math.log10(k);
399
400
401 // received power in dBm's follows the equation:
402 // rxPower = pathLossExponent * kdB + txPower - 5.0 * pathLossExponent * logSqrDistance
403
404 // hence:
405 // 5.0 * pathLossExponent * logSqrDistance = pathLossExponent * kdB + txPower - rxPower
406
407 final var logSqrDistance = (pathLossExponent * kdB + txPower - rxPower) / (5.0 * pathLossExponent);
408
409 // where logSqrDistance = Math.log10(sqrDistance)
410 // and sqrDistance = distance * distance, hence
411 // logSqrDistance = Math.log10(distance * distance) = 2 * Math.log10(distance)
412
413 return Math.pow(10.0, logSqrDistance / 2.0);
414 }
415
416 /**
417 * Obtains distance and its standard deviation for a ranging reading.
418 *
419 * @param reading a ranging reading.
420 * @param positionStandardDeviation position standard deviation, or null if
421 * not available.
422 * @param result array containing distance and its standard
423 * deviation, in such order.
424 */
425 private static void computeDistanceAndStandardDeviationRanging(
426 final RangingReading<? extends RadioSource> reading, final Double positionStandardDeviation,
427 final Double[] result) {
428 computeDistanceAndStandardDeviationRanging(reading.getDistance(), reading.getDistanceStandardDeviation(),
429 positionStandardDeviation, result);
430 }
431
432 /**
433 * Obtains distance and its standard deviation for a ranging reading.
434 *
435 * @param reading a ranging reading.
436 * @param positionStandardDeviation position standard deviation, or null if
437 * not available
438 * @param result array containing distance and its standard
439 * deviation, in such order.
440 */
441 private static void computeDistanceAndStandardDeviationRanging(
442 final RangingAndRssiReading<? extends RadioSource> reading, final Double positionStandardDeviation,
443 final Double[] result) {
444 computeDistanceAndStandardDeviationRanging(reading.getDistance(), reading.getDistanceStandardDeviation(),
445 positionStandardDeviation, result);
446 }
447
448 /**
449 * Obtains distance and its standard deviation.
450 *
451 * @param distance a distance.
452 * @param distanceStandardDeviation distance standard deviation.
453 * @param positionStandardDeviation position standard deviation.
454 * @param result array containing distance and its standard
455 * deviation, in such order.
456 */
457 private static void computeDistanceAndStandardDeviationRanging(
458 final double distance, final Double distanceStandardDeviation, final Double positionStandardDeviation,
459 final Double[] result) {
460 result[0] = distance;
461
462 if (positionStandardDeviation != null || distanceStandardDeviation != null) {
463 var variance = 0.0;
464 if (positionStandardDeviation != null) {
465 variance += positionStandardDeviation * positionStandardDeviation;
466 }
467 if (distanceStandardDeviation != null) {
468 variance += distanceStandardDeviation * distanceStandardDeviation;
469 }
470 result[1] = Math.sqrt(variance);
471 } else {
472 result[1] = null;
473 }
474 }
475
476 /**
477 * Obtains distance and its standard deviation for an RSSI reading.
478 *
479 * @param locatedSource a located source, that must also have power
480 * information.
481 * @param reading an RSSI reading.
482 * @param positionStandardDeviation position standard deviation, or null if not
483 * available.
484 * @param result array containing distance and its standard
485 * deviation, in such order.
486 * @param <P> a {@link Point} type.
487 */
488 private static <P extends Point<?>> void computeDistanceAndStandardDeviationRssi(
489 final RadioSourceLocated<P> locatedSource, final RssiReading<? extends RadioSource> reading,
490 final Double positionStandardDeviation, final Double[] result) {
491 computeDistanceAndStandardDeviationRssi(locatedSource, reading.getRssi(), reading.getRssiStandardDeviation(),
492 positionStandardDeviation, result);
493 }
494
495 /**
496 * Obtains distance and its standard deviation for a ranging and RSSI reading.
497 *
498 * @param locatedSource a located source, that must also have power
499 * information.
500 * @param reading a ranging and RSSI reading.
501 * @param positionStandardDeviation position standard deviation, or null if not
502 * available.
503 * @param result array containing distance and its standard
504 * deviation, in such order.
505 * @param <P> a {@link Point} type.
506 */
507 private static <P extends Point<?>> void computeDistanceAndStandardDeviationRssi(
508 final RadioSourceLocated<P> locatedSource, final RangingAndRssiReading<? extends RadioSource> reading,
509 final Double positionStandardDeviation, final Double[] result) {
510 computeDistanceAndStandardDeviationRssi(locatedSource, reading.getRssi(), reading.getRssiStandardDeviation(),
511 positionStandardDeviation, result);
512 }
513
514 /**
515 * Obtains distance and its standard deviation.
516 *
517 * @param locatedSource a located source, that must also have power
518 * information.
519 * @param rxPower received power expressed in dBm's.
520 * @param rxPowerStandardDeviation received power standard deviation.
521 * @param positionStandardDeviation position standard deviation.
522 * @param result array containing distance and its standard
523 * deviation, in such order.
524 * @param <P> a {@link Point} type.
525 */
526 private static <P extends Point<?>> void computeDistanceAndStandardDeviationRssi(
527 final RadioSourceLocated<P> locatedSource, final double rxPower, final Double rxPowerStandardDeviation,
528 final Double positionStandardDeviation, final Double[] result) {
529
530 if (!(locatedSource instanceof RadioSourceWithPower poweredSource)) {
531 return;
532 }
533
534 // source related parameters
535
536 // transmitted power in dBm's
537 final var txPower = poweredSource.getTransmittedPower();
538 final var txPowerStandardDeviation = poweredSource.getTransmittedPowerStandardDeviation();
539
540 // path loss exponent
541 final var pathLossExponent = poweredSource.getPathLossExponent();
542 final var pathLossExponentStandardDeviation = poweredSource.getPathLossExponentStandardDeviation();
543
544 // WARNING: covariance between tx power and path loss exponent is ignored
545
546 final var frequency = poweredSource.getFrequency();
547
548 final var txPowerVariance = txPowerStandardDeviation != null
549 ? txPowerStandardDeviation * txPowerStandardDeviation : 0.0;
550 final var rxPowerVariance = rxPowerStandardDeviation != null
551 ? rxPowerStandardDeviation * rxPowerStandardDeviation : 0.0;
552 final var pathLossVariance = pathLossExponentStandardDeviation != null
553 ? pathLossExponentStandardDeviation * pathLossExponentStandardDeviation : 0.0;
554
555 var distanceVariance = 0.0;
556 try {
557 final var dist = Utils.propagateVariancesToDistanceVariance(txPower, rxPower, pathLossExponent, frequency,
558 txPowerVariance, rxPowerVariance, pathLossVariance);
559 // distance
560 result[0] = dist.getMean()[0];
561
562 // distance variance
563 distanceVariance = dist.getCovariance().getElementAt(0, 0);
564 } catch (final IndoorException e) {
565 result[0] = computeDistanceRssi(locatedSource, rxPower);
566 if (rxPowerStandardDeviation != null) {
567 // take into account only received power standard deviation
568 distanceVariance = Utils.propagatePowerVarianceToDistanceVariance(txPower, rxPower, pathLossExponent,
569 frequency, rxPowerVariance);
570 }
571 }
572
573 if (txPowerStandardDeviation == null && rxPowerStandardDeviation == null
574 && pathLossExponentStandardDeviation == null && positionStandardDeviation == null) {
575 result[1] = null;
576 } else {
577 if (positionStandardDeviation != null) {
578 distanceVariance += positionStandardDeviation * positionStandardDeviation;
579 }
580 result[1] = Math.sqrt(distanceVariance);
581 }
582 }
583 }