1 /*
2 * Copyright (C) 2019 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.fingerprint;
17
18 import com.irurueta.algebra.AlgebraException;
19 import com.irurueta.algebra.Matrix;
20 import com.irurueta.geometry.Point;
21 import com.irurueta.geometry.Point2D;
22 import com.irurueta.geometry.Point3D;
23 import com.irurueta.navigation.LockedException;
24 import com.irurueta.navigation.NotReadyException;
25 import com.irurueta.navigation.indoor.*;
26 import com.irurueta.numerical.NumericalException;
27 import com.irurueta.numerical.fitting.FittingException;
28 import com.irurueta.numerical.fitting.LevenbergMarquardtMultiDimensionFitter;
29 import com.irurueta.numerical.fitting.LevenbergMarquardtMultiDimensionFunctionEvaluator;
30
31 import java.util.ArrayList;
32 import java.util.Arrays;
33 import java.util.Collection;
34 import java.util.HashMap;
35 import java.util.List;
36
37 /**
38 * Base class for position and radio source estimators based only on located
39 * fingerprints containing RSSI readings.
40 * All implementations of this class estimate the position of a new fingerprint
41 * and the position of all radio sources associated to fingerprints whose location
42 * is known.
43 * All implementations solve the problem in a non-linear way using Levenberg-Marquardt
44 * algorithm.
45 */
46 public abstract class NonLinearFingerprintPositionAndRadioSourceEstimator<P extends Point<?>> extends
47 FingerprintPositionAndRadioSourceEstimator<P> {
48
49 /**
50 * Default RSSI standard deviation assumed for provided fingerprints as a fallback
51 * when none can be determined.
52 */
53 public static final double FALLBACK_RSSI_STANDARD_DEVIATION = 1e-3;
54
55 /**
56 * Indicates that by default measured RSSI standard deviation of closest fingerprint
57 * must be propagated into measured RSSI reading variance at unknown location.
58 */
59 public static final boolean DEFAULT_PROPAGATE_FINGERPRINT_RSSI_STANDARD_DEVIATION = true;
60
61 /**
62 * Indicates that by default path-loss exponent standard deviation of radio source
63 * must be propagated into measured RSSI reading variance at unknown location.
64 */
65 public static final boolean DEFAULT_PROPAGATE_PATHLOSS_EXPONENT_STANDARD_DEVIATION = true;
66
67 /**
68 * Indicates that by default covariance of closest fingerprint position must be
69 * propagated into measured RSSI reading variance at unknown location.
70 */
71 public static final boolean DEFAULT_PROPAGATE_FINGERPRINT_POSITION_COVARIANCE = true;
72
73 /**
74 * Indicates that by default covariance of radio source position must be propagated
75 * into measured RSSI reading variance at unknown location.
76 */
77 public static final boolean DEFAULT_PROPAGATE_RADIO_SOURCE_POSITION_COVARIANCE = true;
78
79 /**
80 * Small value to be used as machine precision.
81 */
82 private static final double TINY = 1e-12;
83
84 /**
85 * Initial sources whose location is known.
86 * If provided, their location will be used as initial values, but
87 * after executing this estimator they will be refined.
88 */
89 protected List<? extends RadioSourceLocated<P>> mInitialLocatedSources;
90
91 /**
92 * Initial position to start the estimation.
93 * This should be a value close to the expected solution.
94 * If no value is provided, the average position among all selected nearest
95 * located fingerprints will be used.
96 */
97 private P initialPosition;
98
99 /**
100 * Indicates whether path loss exponent of provided sources must be used when
101 * available (if true), or if fallback path loss exponent must be used instead.
102 */
103 protected boolean useSourcesPathLossExponentWhenAvailable = true;
104
105 /**
106 * RSSI standard deviation fallback value to use when none can be
107 * determined from provided readings. This fallback value is only used if
108 * no variance is propagated or the resulting value is too small to allow
109 * convergence to a solution.
110 */
111 private double fallbackRssiStandardDeviation = FALLBACK_RSSI_STANDARD_DEVIATION;
112
113 /**
114 * Indicates whether measured RSSI standard deviation of closest fingerprint must
115 * be propagated into measured RSSI reading variance at unknown location.
116 */
117 private boolean propagateFingerprintRssiStandardDeviation = DEFAULT_PROPAGATE_FINGERPRINT_RSSI_STANDARD_DEVIATION;
118
119 /**
120 * Indicates whether path-loss exponent standard deviation of radio source must
121 * be propagated into measured RSSI reading variance at unknown location.
122 */
123 private boolean propagatePathlossExponentStandardDeviation = DEFAULT_PROPAGATE_PATHLOSS_EXPONENT_STANDARD_DEVIATION;
124
125 /**
126 * Indicates whether covariance of closest fingerprint position must be
127 * propagated into measured RSSI reading variance at unknown location.
128 */
129 private boolean propagateFingerprintPositionCovariance = DEFAULT_PROPAGATE_FINGERPRINT_POSITION_COVARIANCE;
130
131 /**
132 * Indicates whether covariance of radio source position must be propagated
133 * into measured RSSI reading variance at unknown location.
134 */
135 private boolean propagateRadioSourcePositionCovariance = DEFAULT_PROPAGATE_RADIO_SOURCE_POSITION_COVARIANCE;
136
137 /**
138 * Levenberg-Marquardt fitter to find a non-linear solution.
139 */
140 private final LevenbergMarquardtMultiDimensionFitter fitter = new LevenbergMarquardtMultiDimensionFitter();
141
142 /**
143 * Estimated covariance matrix for estimated non-located fingerprint position and
144 * estimated located radio sources position.
145 */
146 private Matrix covariance;
147
148 /**
149 * Covariance of estimated position for non-located fingerprint.
150 */
151 private Matrix estimatedPositionCovariance;
152
153 /**
154 * Estimated chi square value.
155 */
156 private double chiSq;
157
158 /**
159 * Constructor.
160 */
161 protected NonLinearFingerprintPositionAndRadioSourceEstimator() {
162 }
163
164 /**
165 * Constructor.
166 *
167 * @param listener listener in charge of handling events.
168 */
169 protected NonLinearFingerprintPositionAndRadioSourceEstimator(
170 final FingerprintPositionAndRadioSourceEstimatorListener<P> listener) {
171 super(listener);
172 }
173
174 /**
175 * Constructor.
176 *
177 * @param locatedFingerprints located fingerprints containing RSSI readings.
178 * @param fingerprint fingerprint containing readings at an unknown location
179 * for provided located fingerprints.
180 * @throws IllegalArgumentException if either non located fingerprint or located
181 * fingerprints are null.
182 */
183 protected NonLinearFingerprintPositionAndRadioSourceEstimator(
184 final List<? extends RssiFingerprintLocated<? extends RadioSource,
185 ? extends RssiReading<? extends RadioSource>, P>> locatedFingerprints,
186 final RssiFingerprint<? extends RadioSource,
187 ? extends RssiReading<? extends RadioSource>> fingerprint) {
188 super(locatedFingerprints, fingerprint);
189 }
190
191 /**
192 * Constructor.
193 *
194 * @param locatedFingerprints located fingerprints containing RSSI readings.
195 * @param fingerprint fingerprint containing readings at an unknown location
196 * for provided located fingerprints.
197 * @param listener listener in charge of handling events.
198 * @throws IllegalArgumentException if either non located fingerprint or located
199 * fingerprints are null.
200 */
201 protected NonLinearFingerprintPositionAndRadioSourceEstimator(
202 final List<? extends RssiFingerprintLocated<? extends RadioSource,
203 ? extends RssiReading<? extends RadioSource>, P>> locatedFingerprints,
204 final RssiFingerprint<? extends RadioSource,
205 ? extends RssiReading<? extends RadioSource>> fingerprint,
206 final FingerprintPositionAndRadioSourceEstimatorListener<P> listener) {
207 super(locatedFingerprints, fingerprint, listener);
208 }
209
210 /**
211 * Constructor.
212 *
213 * @param locatedFingerprints located fingerprints containing RSSI readings.
214 * @param fingerprint fingerprint containing readings at an unknown location
215 * for provided located fingerprints.
216 * @param initialPosition initial position to be assumed on non located fingerprint or
217 * null if unknown.
218 * @throws IllegalArgumentException if either non located fingerprint or located
219 * fingerprints are null.
220 */
221 protected NonLinearFingerprintPositionAndRadioSourceEstimator(
222 final List<? extends RssiFingerprintLocated<? extends RadioSource,
223 ? extends RssiReading<? extends RadioSource>, P>> locatedFingerprints,
224 final RssiFingerprint<? extends RadioSource,
225 ? extends RssiReading<? extends RadioSource>> fingerprint,
226 final P initialPosition) {
227 super(locatedFingerprints, fingerprint);
228 this.initialPosition = initialPosition;
229 }
230
231 /**
232 * Constructor.
233 *
234 * @param locatedFingerprints located fingerprints containing RSSI readings.
235 * @param fingerprint fingerprint containing readings at an unknown location
236 * for provided located fingerprints.
237 * @param initialPosition initial position to be assumed on non located fingerprint or
238 * null if unknown.
239 * @param listener listener in charge of handling events.
240 * @throws IllegalArgumentException if either non located fingerprint or located
241 * fingerprints are null.
242 */
243 protected NonLinearFingerprintPositionAndRadioSourceEstimator(
244 final List<? extends RssiFingerprintLocated<? extends RadioSource,
245 ? extends RssiReading<? extends RadioSource>, P>> locatedFingerprints,
246 final RssiFingerprint<? extends RadioSource,
247 ? extends RssiReading<? extends RadioSource>> fingerprint,
248 final P initialPosition,
249 final FingerprintPositionAndRadioSourceEstimatorListener<P> listener) {
250 super(locatedFingerprints, fingerprint, listener);
251 this.initialPosition = initialPosition;
252 }
253
254 /**
255 * Constructor.
256 *
257 * @param locatedFingerprints located fingerprints containing RSSI readings.
258 * @param fingerprint fingerprint containing readings at an unknown location
259 * for provided located fingerprints.
260 * @param initialLocatedSources sources containing initial location to be refined or null
261 * if unknown.
262 * @throws IllegalArgumentException if either non located fingerprint or located
263 * fingerprints are null.
264 */
265 protected NonLinearFingerprintPositionAndRadioSourceEstimator(
266 final List<? extends RssiFingerprintLocated<? extends RadioSource,
267 ? extends RssiReading<? extends RadioSource>, P>> locatedFingerprints,
268 final RssiFingerprint<? extends RadioSource,
269 ? extends RssiReading<? extends RadioSource>> fingerprint,
270 final List<? extends RadioSourceLocated<P>> initialLocatedSources) {
271 super(locatedFingerprints, fingerprint);
272 mInitialLocatedSources = initialLocatedSources;
273 }
274
275 /**
276 * Constructor.
277 *
278 * @param locatedFingerprints located fingerprints containing RSSI readings.
279 * @param fingerprint fingerprint containing readings at an unknown location
280 * for provided located fingerprints.
281 * @param initialLocatedSources sources containing initial location to be refined or null
282 * if unknown.
283 * @param listener listener in charge of handling events.
284 * @throws IllegalArgumentException if either non located fingerprint or located
285 * fingerprints are null.
286 */
287 protected NonLinearFingerprintPositionAndRadioSourceEstimator(
288 final List<? extends RssiFingerprintLocated<? extends RadioSource,
289 ? extends RssiReading<? extends RadioSource>, P>> locatedFingerprints,
290 final RssiFingerprint<? extends RadioSource,
291 ? extends RssiReading<? extends RadioSource>> fingerprint,
292 final List<? extends RadioSourceLocated<P>> initialLocatedSources,
293 final FingerprintPositionAndRadioSourceEstimatorListener<P> listener) {
294 super(locatedFingerprints, fingerprint, listener);
295 mInitialLocatedSources = initialLocatedSources;
296 }
297
298 /**
299 * Constructor.
300 *
301 * @param locatedFingerprints located fingerprints containing RSSI readings.
302 * @param fingerprint fingerprint containing readings at an unknown location
303 * for provided located fingerprints.
304 * @param initialPosition initial position to be assumed on non located fingerprint or
305 * null if unknown.
306 * @param initialLocatedSources sources containing initial location to be refined or null
307 * if unknown.
308 * @throws IllegalArgumentException if either non located fingerprint or located
309 * fingerprints are null.
310 */
311 protected NonLinearFingerprintPositionAndRadioSourceEstimator(
312 final List<? extends RssiFingerprintLocated<? extends RadioSource,
313 ? extends RssiReading<? extends RadioSource>, P>> locatedFingerprints,
314 final RssiFingerprint<? extends RadioSource,
315 ? extends RssiReading<? extends RadioSource>> fingerprint,
316 final P initialPosition,
317 final List<? extends RadioSourceLocated<P>> initialLocatedSources) {
318 this(locatedFingerprints, fingerprint, initialPosition);
319 mInitialLocatedSources = initialLocatedSources;
320 }
321
322 /**
323 * Constructor.
324 *
325 * @param locatedFingerprints located fingerprints containing RSSI readings.
326 * @param fingerprint fingerprint containing readings at an unknown location
327 * for provided located fingerprints.
328 * @param initialPosition initial position to be assumed on non located fingerprint or
329 * null if unknown.
330 * @param initialLocatedSources sources containing initial location to be refined or null
331 * if unknown.
332 * @param listener listener in charge of handling events.
333 * @throws IllegalArgumentException if either non located fingerprint or located
334 * fingerprints are null.
335 */
336 protected NonLinearFingerprintPositionAndRadioSourceEstimator(
337 final List<? extends RssiFingerprintLocated<? extends RadioSource,
338 ? extends RssiReading<? extends RadioSource>, P>> locatedFingerprints,
339 final RssiFingerprint<? extends RadioSource,
340 ? extends RssiReading<? extends RadioSource>> fingerprint,
341 final P initialPosition,
342 final List<? extends RadioSourceLocated<P>> initialLocatedSources,
343 final FingerprintPositionAndRadioSourceEstimatorListener<P> listener) {
344 this(locatedFingerprints, fingerprint, initialPosition, listener);
345 mInitialLocatedSources = initialLocatedSources;
346 }
347
348 /**
349 * Gets initial radio sources whose location is known.
350 *
351 * @return initial radio sources.
352 */
353 public List<RadioSourceLocated<P>> getInitialLocatedSources() {
354 //noinspection unchecked
355 return (List<RadioSourceLocated<P>>) mInitialLocatedSources;
356 }
357
358 /**
359 * Sets initial radio sources whose location is known.
360 *
361 * @param initialLocatedSources initial radio sources.
362 * @throws LockedException if estimator is locked.
363 */
364 public void setInitialLocatedSources(final List<? extends RadioSourceLocated<P>> initialLocatedSources)
365 throws LockedException {
366 if (isLocked()) {
367 throw new LockedException();
368 }
369
370 mInitialLocatedSources = initialLocatedSources;
371 }
372
373 /**
374 * Gets initial position to start the solving algorithm.
375 * This should be a value close to the expected solution.
376 * If no value is provided, the average position among all selected nearest
377 * located fingerprints will be used.
378 *
379 * @return initial position to start the solving algorithm or null.
380 */
381 public P getInitialPosition() {
382 return initialPosition;
383 }
384
385 /**
386 * Sets initial position to start the solving algorithm.
387 * This should be a value close to the expected solution.
388 * If no value is provided, the average position among all selected nearest
389 * located fingerprints will be used.
390 *
391 * @param initialPosition initial position to start the solving algorithm or null.
392 * @throws LockedException if estimator is locked.
393 */
394 public void setInitialPosition(final P initialPosition) throws LockedException {
395 if (isLocked()) {
396 throw new LockedException();
397 }
398
399 this.initialPosition = initialPosition;
400 }
401
402 /**
403 * Gets estimated covariance matrix for estimated non-located fingerprint position
404 * and estimated located radio sources position.
405 *
406 * @return estimated covariance matrix for estimated non-located fingerprint
407 * position and estimated located radio sources position.
408 */
409 public Matrix getCovariance() {
410 return covariance;
411 }
412
413 /**
414 * Gets covariance of estimated position for non-located fingerprint.
415 *
416 * @return covariance of estimated position for non-located fingerprint.
417 */
418 public Matrix getEstimatedPositionCovariance() {
419 return estimatedPositionCovariance;
420 }
421
422 /**
423 * Gets estimated chi square value.
424 *
425 * @return estimated chi square value.
426 */
427 public double getChiSq() {
428 return chiSq;
429 }
430
431 /**
432 * Indicates whether path loss exponent of provided sources must be used when
433 * available (if true), or if fallback path loss exponent must be used instead.
434 *
435 * @return true to use path loss exponent of provided sources when available,
436 * false otherwise.
437 */
438 public boolean getUseSourcesPathLossExponentWhenAvailable() {
439 return useSourcesPathLossExponentWhenAvailable;
440 }
441
442 /**
443 * Specifies whether path loss exponent of provided sources must be used when
444 * available (if true), or if fallback path loss exponent must be used instead.
445 *
446 * @param useSourcesPathLossExponentWhenAvailable true to use path loss exponent of
447 * provided sources when available,
448 * false otherwise.
449 * @throws LockedException if estimator is locked.
450 */
451 public void setUseSourcesPathLossExponentWhenAvailable(final boolean useSourcesPathLossExponentWhenAvailable)
452 throws LockedException {
453 if (isLocked()) {
454 throw new LockedException();
455 }
456 this.useSourcesPathLossExponentWhenAvailable = useSourcesPathLossExponentWhenAvailable;
457 }
458
459 /**
460 * Gets RSSI standard deviation fallback value to use when none can be
461 * determined from provided readings.
462 *
463 * @return RSSI standard deviation fallback.
464 */
465 public double getFallbackRssiStandardDeviation() {
466 return fallbackRssiStandardDeviation;
467 }
468
469 /**
470 * Sets RSSI standard deviation fallback value to use when none can be
471 * determined from provided readings.
472 *
473 * @param fallbackRssiStandardDeviation RSSI standard deviation fallback
474 * @throws LockedException if estimator is locked.
475 * @throws IllegalArgumentException if provided value is smaller than
476 * {@link #TINY}.
477 */
478 public void setFallbackRssiStandardDeviation(final double fallbackRssiStandardDeviation) throws LockedException {
479 if (isLocked()) {
480 throw new LockedException();
481 }
482 if (fallbackRssiStandardDeviation < TINY) {
483 throw new IllegalArgumentException();
484 }
485 this.fallbackRssiStandardDeviation = fallbackRssiStandardDeviation;
486 }
487
488 /**
489 * Indicates whether measured RSSI standard deviation of closest fingerprint must be
490 * propagated into measured RSSI reading variance at unknown location.
491 *
492 * @return true to propagate RSSI standard deviation of closest fingerprint,
493 * false otherwise.
494 */
495 public boolean isFingerprintRssiStandardDeviationPropagated() {
496 return propagateFingerprintRssiStandardDeviation;
497 }
498
499 /**
500 * Specifies whether measured RSSI standard deviation of closest fingerprint must be
501 * propagated into measured RSSI reading variance at unknown location.
502 *
503 * @param propagateFingerprintRssiStandardDeviation true to propagate RSSI standard
504 * deviation of closest fingerprint,
505 * false otherwise.
506 * @throws LockedException if estimator is locked.
507 */
508 public void setFingerprintRssiStandardDeviationPropagated(final boolean propagateFingerprintRssiStandardDeviation)
509 throws LockedException {
510 if (isLocked()) {
511 throw new LockedException();
512 }
513 this.propagateFingerprintRssiStandardDeviation = propagateFingerprintRssiStandardDeviation;
514 }
515
516 /**
517 * Indicates whether path-loss exponent standard deviation of radio source must be
518 * propagated into measured RSSI reading variance at unknown location.
519 *
520 * @return true to propagate path-loss exponent standard deviation of radio source,
521 * false otherwise.
522 */
523 public boolean isPathlossExponentStandardDeviationPropagated() {
524 return propagatePathlossExponentStandardDeviation;
525 }
526
527 /**
528 * Specifies whether path-loss exponent standard deviation of radio source must be
529 * propagated into measured RSSI reading variance at unknown location.
530 *
531 * @param propagatePathlossExponentStandardDeviation true to propagate path-loss
532 * exponent standard deviation of
533 * radio source, false otherwise.
534 * @throws LockedException if estimator is locked.
535 */
536 public void setPathlossExponentStandardDeviationPropagated(
537 final boolean propagatePathlossExponentStandardDeviation) throws LockedException {
538 if (isLocked()) {
539 throw new LockedException();
540 }
541 this.propagatePathlossExponentStandardDeviation = propagatePathlossExponentStandardDeviation;
542 }
543
544 /**
545 * Indicates whether covariance of closest fingerprint position must be propagated
546 * into measured RSSI reading variance at unknown location.
547 *
548 * @return true to propagate fingerprint position covariance, false otherwise.
549 */
550 public boolean isFingerprintPositionCovariancePropagated() {
551 return propagateFingerprintPositionCovariance;
552 }
553
554 /**
555 * Specifies whether covariance of closest fingerprint position must be propagated
556 * into measured RSSI reading variance at unknown location.
557 *
558 * @param propagateFingerprintPositionCovariance true to propagate fingerprint
559 * position covariance, false otherwise.
560 * @throws LockedException if estimator is locked.
561 */
562 public void setFingerprintPositionCovariancePropagated(final boolean propagateFingerprintPositionCovariance)
563 throws LockedException {
564 if (isLocked()) {
565 throw new LockedException();
566 }
567 this.propagateFingerprintPositionCovariance = propagateFingerprintPositionCovariance;
568 }
569
570 /**
571 * Indicates whether covariance of radio source position must be propagated into
572 * measured RSSI reading variance at unknown location.
573 *
574 * @return true to propagate radio source position covariance, false otherwise.
575 */
576 public boolean isRadioSourcePositionCovariancePropagated() {
577 return propagateRadioSourcePositionCovariance;
578 }
579
580 /**
581 * Specifies whether covariance of radio source position must be propagated into
582 * measured RSSI reading variance at unknown location.
583 *
584 * @param propagateRadioSourcePositionCovariance true to propagate radio source
585 * position covariance, false otherwise.
586 * @throws LockedException if estimator is locked.
587 */
588 public void setRadioSourcePositionCovariancePropagated(final boolean propagateRadioSourcePositionCovariance)
589 throws LockedException {
590 if (isLocked()) {
591 throw new LockedException();
592 }
593 this.propagateRadioSourcePositionCovariance = propagateRadioSourcePositionCovariance;
594 }
595
596 /**
597 * Estimates position and radio sources based on provided located radio sources and readings of
598 * such radio sources at an unknown location.
599 *
600 * @throws LockedException if estimator is locked.
601 * @throws NotReadyException if estimator is not ready.
602 * @throws FingerprintEstimationException if estimation fails for some other reason.
603 */
604 @Override
605 @SuppressWarnings("Duplicates")
606 public void estimate() throws LockedException, NotReadyException, FingerprintEstimationException {
607
608 if (!isReady()) {
609 throw new NotReadyException();
610 }
611 if (isLocked()) {
612 throw new LockedException();
613 }
614
615 try {
616 locked = true;
617
618 if (listener != null) {
619 listener.onEstimateStart(this);
620 }
621
622 RadioSourceNoMeanKNearestFinder<P, RadioSource> noMeanFinder = null;
623 RadioSourceKNearestFinder<P, RadioSource> finder = null;
624 if (useNoMeanNearestFingerprintFinder) {
625 //noinspection unchecked
626 noMeanFinder = new RadioSourceNoMeanKNearestFinder<>(
627 (Collection<RssiFingerprintLocated<RadioSource,
628 RssiReading<RadioSource>, P>>) locatedFingerprints);
629 } else {
630 //noinspection unchecked
631 finder = new RadioSourceKNearestFinder<>(
632 (Collection<RssiFingerprintLocated<RadioSource,
633 RssiReading<RadioSource>, P>>) locatedFingerprints);
634 }
635
636 estimatedPositionCoordinates = null;
637 covariance = null;
638 estimatedPositionCovariance = null;
639 nearestFingerprints = null;
640 estimatedLocatedSources = null;
641
642 final var min = Math.max(1, minNearestFingerprints);
643 final var max = maxNearestFingerprints < 0
644 ? locatedFingerprints.size()
645 : Math.min(maxNearestFingerprints, locatedFingerprints.size());
646 for (var k = min; k <= max; k++) {
647 if (noMeanFinder != null) {
648 //noinspection unchecked
649 nearestFingerprints = noMeanFinder.findKNearestTo(
650 (RssiFingerprint<RadioSource, RssiReading<RadioSource>>) fingerprint, k);
651 } else {
652 //noinspection unchecked
653 nearestFingerprints = finder.findKNearestTo(
654 (RssiFingerprint<RadioSource, RssiReading<RadioSource>>) fingerprint, k);
655 }
656
657 // Demonstration in 2D:
658 // --------------------
659
660 // The expression of received power expressed in dBm's is:
661 // k = (c/(4*pi*f))
662 // Pr = Pte*k^n / d^n
663
664 // where c is the speed of light, pi is 3.14159..., f is the frequency of the radio source,
665 // Pte is the equivalent transmitted power by the radio source, n is the path-loss exponent
666 // (typically 2.0), and d is the distance from a point to the location of the radio source.
667
668 // Hence:
669 // Pr(dBm) = 10*log(Pte*k^n/d^n) = 10*n*log(k) + 10*log(Pte) - 10*n*log(d) =
670 // 10*n*log(k) + 10*log(Pte) - 5*n*log(d^2)
671
672 // where d^2 = dia^2 = (xi - xa)^2 + (yi - ya)^2 is the squared distance between
673 // fingerprint and unknown point pi = (xi, yi) and
674 // radio source a = a,b... M
675 // 10*n*log(k) is constant for a given radio source "a", and
676 // Pte is the equivalent transmitted power of radio source "a".
677
678 // We assume that the 2 former terms are constant and known for a given radio source
679 // K = 10*n*log(k) + 10*log(Pte), and the only unknown term is
680 // the latter one depending on the distance of the
681 // measuring point and the radio source.
682
683 // Hence for a given radio source "a" at unknown location "i":
684 // Pr(pi) = K - 5*n*log((xi - xa)^2 + (yi - ya)^2)
685
686 // we assume that a known located fingerprint is located at p1 = (x1, y1),
687 // Both readings Pr(pi) and Pr(p1) belong to the same radio source "a", hence
688 // K term is the same.
689
690 // Pr(p1) = K - 5*n*log((x1 - xa)^2 + (y1 - ya)^2)
691
692 // where d1a^2 = (x1 - xa)^2 + (y1 - ya)^2 is the squared distance between
693 // fingerprint 1 and radio source 2
694
695 // To remove possible bias effects on readings, we consider the difference of received
696 // power for fingerprint "1" and radio source "a" as:
697
698 // Prdiff1a = Pr(pi) - Pr(p1) = (K - 5*n*log(dia^2)) - (K - 5*n*log(d1a^2)) =
699 // = 5*n*log(d1a^2) - 5*n*log(dia^2)
700
701 // where both d1a^2 and dia^2 are unknown, because location pi=(xi,yi) and pa=(xa,ya) are unknown.
702 // Now we have no dependencies on the amount of transmitted power of each radio source
703 // contained on constant term K, and we only depend on squared distances d1a^2 and dia^2.
704
705 // Consequently, the difference of received power for fingerprint "2" and radio source "a" is:
706 // Prdiff2a = 5*n*log(d2a^2) - 5*n*log(dia^2)
707
708 // the difference of received power for fingerprint "1" and radio source "b" is:
709 // Prdiff1b = 5*n*log(d1b^2) - 5*n*log(dib^2)
710
711 // and so on.
712
713 // we want to find unknown location pi, and location of radio source pa, pb,... pM so that Prdiff
714 // errors are minimized in LMSE (Least Mean Square Error) terms.
715
716 // Assuming that we have M radio sources and N fingerprints, we have
717 // y = [Prdiff1a Prdiff2a Prdiff1b Prdiff2b ... PrdiffNa PrdiffNb ... PrdiffNM]
718
719 // and the unknowns to be found are:
720 // x = [xi yi xa ya xb yb ... xM yM], which are the location of the unknown fingerprint
721 // pi = (xi, yi) and the locations of the radio sources a, b ... M that we want to find pa = (xa, ya),
722 // pb = (xb, yb) ... pM = (xM, yM)
723
724 try {
725 final var sourcesToBeEstimated = setupFitter();
726 if (minNearestFingerprints < 0) {
727 // if no limit is set in minimum value, then a minimum of
728 // dims * (1 + numSources) is used
729 final var numSources = sourcesToBeEstimated.size();
730 final var dims = getNumberOfDimensions();
731 final var minNearest = dims * (1 + numSources);
732 if (k < minNearest) {
733 continue;
734 }
735 }
736
737 fitter.fit();
738
739 // estimated position
740 final var a = fitter.getA();
741 covariance = fitter.getCovar();
742 chiSq = fitter.getChisq();
743
744 final var dims = getNumberOfDimensions();
745
746 // obtain estimated position coordinates and covariance
747 estimatedPositionCoordinates = new double[dims];
748 System.arraycopy(a, 0, estimatedPositionCoordinates, 0, dims);
749
750 final var dimsMinusOne = dims - 1;
751 estimatedPositionCovariance = covariance.getSubmatrix(0, 0, dimsMinusOne,
752 dimsMinusOne);
753
754 // obtain radio sources estimated positions and covariance
755 final var totalSources = sourcesToBeEstimated.size();
756 estimatedLocatedSources = new ArrayList<>();
757 for (var j = 0; j < totalSources; j++) {
758 final var sourcePosition = createPoint();
759
760 final var start = dims * (1 + j);
761 final var end = start + dimsMinusOne;
762 for (var i = 0; i < dims; i++) {
763 sourcePosition.setInhomogeneousCoordinate(i, a[start + i]);
764 }
765
766 final var sourceCovariance = covariance.getSubmatrix(start, start, end, end);
767
768 final var source = sourcesToBeEstimated.get(j);
769 estimatedLocatedSources.add(createRadioSource(source, sourcePosition, sourceCovariance));
770 }
771
772 // a solution was found so we exit loop
773 break;
774 } catch (final NumericalException e) {
775 // solution could not be found with current data
776 // Iterate to use additional nearby fingerprints
777 estimatedPositionCoordinates = null;
778 covariance = null;
779 estimatedPositionCovariance = null;
780 estimatedLocatedSources = null;
781 }
782 }
783
784 if (estimatedPositionCoordinates == null || estimatedLocatedSources == null) {
785 // no position could be estimated
786 throw new FingerprintEstimationException();
787 }
788
789 if (listener != null) {
790 listener.onEstimateEnd(this);
791 }
792 } finally {
793 locked = false;
794 }
795 }
796
797 /**
798 * Propagates provided variances into RSSI differences.
799 *
800 * @param pathlossExponent path-loss exponent.
801 * @param fingerprintPosition position of closest located fingerprint.
802 * @param radioSourcePosition radio source position associated to fingerprint reading.
803 * @param estimatedPosition position to be estimated. Usually this is equal to the
804 * initial position used by a non-linear algorithm.
805 * @param pathlossExponentVariance variance of path-loss exponent or null if unknown.
806 * @param fingerprintPositionCovariance covariance of fingerprint position or null if
807 * unknown.
808 * @param radioSourcePositionCovariance covariance of radio source position or null if
809 * unknown.
810 * @return variance of RSSI difference measured at non located fingerprint reading.
811 */
812 protected abstract Double propagateVariances(
813 final double pathlossExponent, final P fingerprintPosition,
814 final P radioSourcePosition, final P estimatedPosition,
815 final Double pathlossExponentVariance,
816 final Matrix fingerprintPositionCovariance,
817 final Matrix radioSourcePositionCovariance);
818
819 /**
820 * Creates a located radio source from provided radio source, position and
821 * covariance.
822 *
823 * @param source radio source.
824 * @param sourcePosition radio source position.
825 * @param sourceCovariance radio source position covariance.
826 * @return located radio source.
827 */
828 private RadioSourceLocated<P> createRadioSource(
829 final RadioSource source, final P sourcePosition, final Matrix sourceCovariance) {
830
831 final var dims = getNumberOfDimensions();
832
833 switch (source.getType()) {
834 case BEACON:
835 final var beacon = (Beacon) source;
836 if (dims == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH) {
837 // 2D
838
839 //noinspection unchecked
840 return (RadioSourceLocated<P>) new BeaconLocated2D(
841 beacon.getIdentifiers(), beacon.getTransmittedPower(),
842 beacon.getFrequency(), beacon.getBluetoothAddress(),
843 beacon.getBeaconTypeCode(), beacon.getManufacturer(),
844 beacon.getServiceUuid(), beacon.getBluetoothName(),
845 (Point2D) sourcePosition, sourceCovariance);
846 } else {
847 // 3D
848
849 //noinspection unchecked
850 return (RadioSourceLocated<P>) new BeaconLocated3D(
851 beacon.getIdentifiers(), beacon.getTransmittedPower(),
852 beacon.getFrequency(), beacon.getBluetoothAddress(),
853 beacon.getBeaconTypeCode(), beacon.getManufacturer(),
854 beacon.getServiceUuid(), beacon.getBluetoothName(),
855 (Point3D) sourcePosition, sourceCovariance);
856 }
857 case WIFI_ACCESS_POINT:
858 default:
859 final var accessPoint = (WifiAccessPoint) source;
860 if (dims == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH) {
861 // 2D
862
863 //noinspection unchecked
864 return (RadioSourceLocated<P>) new WifiAccessPointLocated2D(
865 accessPoint.getBssid(), accessPoint.getFrequency(),
866 accessPoint.getSsid(), (Point2D) sourcePosition,
867 sourceCovariance);
868 } else {
869 // 3D
870
871 //noinspection unchecked
872 return (RadioSourceLocated<P>) new WifiAccessPointLocated3D(
873 accessPoint.getBssid(), accessPoint.getFrequency(),
874 accessPoint.getSsid(), (Point3D) sourcePosition,
875 sourceCovariance);
876 }
877 }
878 }
879
880 /**
881 * Builds data required to solve the problem.
882 * This method takes into account current nearest fingerprints and discards those
883 * readings belonging to radio sources not having enough data to be estimated.
884 *
885 * @param allPowerDiffs list of received power differences of RSSI readings between a
886 * located fingerprint and an unknown fingerprint for a given radio
887 * source.
888 * @param allFingerprintPositions positions of all located fingerprints being taken into
889 * account.
890 * @param allInitialSourcesPositions initial positions of all radio sources to be taken
891 * into account. If initial located sources where provided,
892 * their positions will be used, otherwise the centroid of
893 * all located fingerprints associated to a radio source will
894 * be used as initial position.
895 * @param allSourcesToBeEstimated all radio sources that will be estimated.
896 * @param allSourcesIndices indices indicating the position radio source being used
897 * within the list of sources for current reading.
898 * @param allPathLossExponents list of path loss exponents.
899 * @param allStandardDeviations list of standard deviations for readings being used.
900 * @param nearestFingerprintsCentroid centroid of nearest fingerprints being taken into account.
901 */
902 @SuppressWarnings("Duplicates")
903 private void buildData(
904 final List<Double> allPowerDiffs,
905 final List<P> allFingerprintPositions,
906 final List<P> allInitialSourcesPositions,
907 final List<RadioSource> allSourcesToBeEstimated,
908 final List<Integer> allSourcesIndices,
909 final List<Double> allPathLossExponents,
910 final List<Double> allStandardDeviations,
911 final P nearestFingerprintsCentroid) {
912
913 final var dims = getNumberOfDimensions();
914 var num = 0;
915 final var centroidCoords = new double[dims];
916 for (final var fingerprint : nearestFingerprints) {
917 final var position = fingerprint.getPosition();
918 if (position == null) {
919 continue;
920 }
921
922 for (var i = 0; i < dims; i++) {
923 centroidCoords[i] += position.getInhomogeneousCoordinate(i);
924 }
925 num++;
926 }
927
928 if (num > 0) {
929 for (var i = 0; i < dims; i++) {
930 centroidCoords[i] /= num;
931 nearestFingerprintsCentroid.setInhomogeneousCoordinate(i, centroidCoords[i]);
932 }
933 }
934
935 // maps to keep cached in memory computed values to speed up computations
936 final var numReadingsMap = new HashMap<RadioSource, Integer>();
937 final var centroidsMap = new HashMap<RadioSource, P>();
938
939 for (final var locatedFingerprint : nearestFingerprints) {
940
941 final var locatedReadings = locatedFingerprint.getReadings();
942 if (locatedReadings == null) {
943 continue;
944 }
945
946 final var fingerprintPosition = locatedFingerprint.getPosition();
947 final var fingerprintPositionCovariance = locatedFingerprint.getPositionCovariance();
948
949 for (final var locatedReading : locatedReadings) {
950 final var source = locatedReading.getSource();
951
952 // obtain the total number of readings available for this source and
953 // the centroid of all located fingerprints containing readings for
954 // such source
955 final int numReadings;
956 if (!numReadingsMap.containsKey(source)) {
957 numReadings = totalReadingsForSource(source, nearestFingerprints, null);
958 numReadingsMap.put(source, numReadings);
959 } else {
960 numReadings = numReadingsMap.get(source);
961 }
962
963 if (numReadings < dims) {
964 continue;
965 }
966
967 final P centroid;
968 if (!centroidsMap.containsKey(source)) {
969 centroid = createPoint();
970
971 //noinspection unchecked
972 totalReadingsForSource(source,
973 (List<RssiFingerprintLocated<RadioSource, RssiReading<RadioSource>, P>>) locatedFingerprints,
974 centroid);
975
976 centroidsMap.put(source, centroid);
977 } else {
978 centroid = centroidsMap.get(source);
979 }
980
981 // find within the list of located sources (if available) the source
982 // of current located fingerprint
983
984 //noinspection SuspiciousMethodCalls
985 final var pos = mInitialLocatedSources != null ? mInitialLocatedSources.indexOf(source) : -1;
986
987 var pathLossExponent = this.pathLossExponent;
988 Double pathLossExponentVariance = null;
989 if (useSourcesPathLossExponentWhenAvailable && source instanceof RadioSourceWithPower sourceWithPower) {
990 pathLossExponent = sourceWithPower.getPathLossExponent();
991 final var std = sourceWithPower.getPathLossExponentStandardDeviation();
992 pathLossExponentVariance = std != null ? std * std : null;
993 }
994
995 final P sourcePosition;
996 Matrix sourcePositionCovariance = null;
997 if (pos < 0) {
998 // located source is not available, so we use centroid
999 sourcePosition = centroid;
1000 } else {
1001 final var locatedSource = mInitialLocatedSources.get(pos);
1002 sourcePosition = locatedSource.getPosition();
1003
1004 if (useSourcesPathLossExponentWhenAvailable
1005 && locatedSource instanceof RadioSourceWithPower locatedSourceWithPower
1006 && pathLossExponentVariance == null) {
1007 pathLossExponent = locatedSourceWithPower.getPathLossExponent();
1008 final var std = locatedSourceWithPower.getPathLossExponentStandardDeviation();
1009 pathLossExponentVariance = std != null ? std * std : null;
1010 }
1011
1012 sourcePositionCovariance = locatedSource.getPositionCovariance();
1013 }
1014
1015 final int sourceIndex;
1016 if (!allSourcesToBeEstimated.contains(source)) {
1017 sourceIndex = allSourcesToBeEstimated.size();
1018
1019 allSourcesToBeEstimated.add(source);
1020 allInitialSourcesPositions.add(sourcePosition);
1021 } else {
1022 sourceIndex = allSourcesToBeEstimated.indexOf(source);
1023 }
1024
1025 final var locatedRssi = locatedReading.getRssi();
1026
1027 final var locatedRssiStd = locatedReading.getRssiStandardDeviation();
1028 final var locatedRssiVariance = locatedRssiStd != null ? locatedRssiStd * locatedRssiStd : null;
1029
1030 final var readings = fingerprint.getReadings();
1031 for (final var reading : readings) {
1032 if (reading.getSource() == null || !reading.getSource().equals(source)) {
1033 continue;
1034 }
1035
1036 // only take into account reading for matching sources on located
1037 // and non-located readings
1038 final var rssi = reading.getRssi();
1039
1040 final var powerDiff = rssi - locatedRssi;
1041
1042 Double standardDeviation = null;
1043 if (propagatePathlossExponentStandardDeviation || propagateFingerprintPositionCovariance
1044 || propagateRadioSourcePositionCovariance) {
1045
1046 // compute initial position
1047 final var initialPosition = this.initialPosition != null
1048 ? this.initialPosition : nearestFingerprintsCentroid;
1049
1050 final var variance = propagateVariances(pathLossExponent,
1051 fingerprintPosition, sourcePosition, initialPosition,
1052 propagatePathlossExponentStandardDeviation ? pathLossExponentVariance : null,
1053 propagateFingerprintPositionCovariance ? fingerprintPositionCovariance : null,
1054 propagateRadioSourcePositionCovariance ? sourcePositionCovariance : null);
1055 if (variance != null) {
1056 standardDeviation = Math.sqrt(variance);
1057 }
1058 }
1059
1060 if (standardDeviation == null) {
1061 standardDeviation = reading.getRssiStandardDeviation();
1062 }
1063
1064 if (propagateFingerprintRssiStandardDeviation) {
1065 if (standardDeviation != null && reading.getRssiStandardDeviation() != null) {
1066 // consider propagated variance and reading variance independent, so we
1067 // sum them both
1068 standardDeviation = standardDeviation * standardDeviation
1069 + reading.getRssiStandardDeviation() * reading.getRssiStandardDeviation();
1070 standardDeviation = Math.sqrt(standardDeviation);
1071 }
1072
1073 if (locatedRssiVariance != null && standardDeviation != null) {
1074 // consider propagated variance and located reading variance
1075 // independent, so we sum them both
1076 standardDeviation = standardDeviation * standardDeviation + locatedRssiVariance;
1077 standardDeviation = Math.sqrt(standardDeviation);
1078 }
1079 }
1080
1081 if (standardDeviation == null || standardDeviation < TINY) {
1082 standardDeviation = fallbackRssiStandardDeviation;
1083 }
1084
1085 allPowerDiffs.add(powerDiff);
1086 allFingerprintPositions.add(fingerprintPosition);
1087 allSourcesIndices.add(sourceIndex);
1088 allPathLossExponents.add(pathLossExponent);
1089 allStandardDeviations.add(standardDeviation);
1090 }
1091 }
1092 }
1093 }
1094
1095 /**
1096 * Setups fitter to solve positions.
1097 *
1098 * @return list of radio sources whose location will be estimated.
1099 * @throws FittingException if Levenberg-Marquardt fitting fails.
1100 */
1101 @SuppressWarnings("Duplicates")
1102 private List<RadioSource> setupFitter() throws FittingException {
1103 // build lists of data
1104 final var allPowerDiffs = new ArrayList<Double>();
1105 final var allFingerprintPositions = new ArrayList<P>();
1106 final var allInitialSourcesPositions = new ArrayList<P>();
1107 final var allSourcesToBeEstimated = new ArrayList<RadioSource>();
1108 final var allSourcesIndices = new ArrayList<Integer>();
1109 final var allPathLossExponents = new ArrayList<Double>();
1110 final var allStandardDeviations = new ArrayList<Double>();
1111 final var nearestFingerprintsCentroid = createPoint();
1112 buildData(allPowerDiffs, allFingerprintPositions, allInitialSourcesPositions, allSourcesToBeEstimated,
1113 allSourcesIndices, allPathLossExponents, allStandardDeviations, nearestFingerprintsCentroid);
1114
1115 final var totalReadings = allPowerDiffs.size();
1116 final var totalSources = allSourcesToBeEstimated.size();
1117 final var dims = getNumberOfDimensions();
1118 final var n = 1 + dims;
1119
1120 fitter.setFunctionEvaluator(new LevenbergMarquardtMultiDimensionFunctionEvaluator() {
1121 @Override
1122 public int getNumberOfDimensions() {
1123 return n;
1124 }
1125
1126 @Override
1127 public double[] createInitialParametersArray() {
1128 final var initial = new double[dims * (totalSources + 1)];
1129
1130 if (initialPosition == null) {
1131 // use centroid of nearest fingerprints as initial value
1132 for (var i = 0; i < dims; i++) {
1133 initial[i] = nearestFingerprintsCentroid.getInhomogeneousCoordinate(i);
1134 }
1135 } else {
1136 // use provided initial position
1137 for (var i = 0; i < dims; i++) {
1138 initial[i] = initialPosition.getInhomogeneousCoordinate(i);
1139 }
1140 }
1141
1142 var pos = dims;
1143 for (var j = 0; j < totalSources; j++) {
1144 final var initialSourcePosition = allInitialSourcesPositions.get(j);
1145 for (var i = 0; i < dims; i++) {
1146 initial[pos] = initialSourcePosition.getInhomogeneousCoordinate(i);
1147 pos++;
1148 }
1149 }
1150
1151 return initial;
1152 }
1153
1154 @Override
1155 public double evaluate(
1156 final int i, final double[] point, final double[] params, final double[] derivatives) {
1157
1158 // For 2D:
1159 // -------
1160
1161 // Prdiff1a = Pr(pi) - Pr(p1) = 5*n*log(d1a^2) - 5*n*log(dia^2) =
1162 // = 5*n*log((x1 - xa)^2 + (y1 - ya)^2) - 5*n*log((xi - xa)^2 + (yi - ya)^2)
1163
1164 // derivatives respect parameters being estimated (xi,yi,xa,ya...,xM,yM)
1165 // for unknown point pi = (xi, yi)
1166 // diff(Prdiff1a)/diff(xi) = -5*n/(log(10)*((xi - xa)^2 + (yi - ya)^2))*2*(xi - xa)
1167 // = -10*n*(xi - xa)/(log(10)*((xi - xa)^2 + (yi - ya)^2))
1168 // = -10*n*(xi - xa)/(log(10)*dia^2)
1169
1170 // diff(Prdiff1a)/diff(yi) = -5*n/(log(10)*((xi - xa)^2 + (yi - ya)^2))*2*(yi - ya)
1171 // = -10*n*(yi - ya)/(log(10)*((xi - xa)^2 + (yi - ya)^2))
1172 // = -10*n*(yi - ya)/(log(10)*dia^2)
1173
1174 // for same radio source pa=(xa,ya)
1175 // diff(Prdiff1a)/diff(xa) = 5*n/(log(10)*((x1 - xa)^2 + (y1 - ya)^2))*-2*(x1 - xa) -5*n/(log(10)*((xi - xa)^2 + (yi - ya)^2))*-2*(xi - xa) =
1176 // = -10*n*(x1 - xa)/(log(10)*((x1 - xa)^2 + (y1 - ya)^2)) + 10*n*(xi - xa)/(log(10)*((xi - xa)^2 + (yi - ya)^2))
1177 // = 10*n*(-(x1 - xa)/(log(10)*d1a^2) + (xi - xa)/(log(10)*dia^2))
1178
1179 // diff(Prdiff1a)/diff(ya) = 5*n/(log(10)*((x1 - xa)^2 + (y1 - ya)^2))*-2*(y1 - ya) -5*n/(log(10)*((xi - xa)^2 + (yi - ya)^2))*-2*(yi - ya) =
1180 // = -10*n*(y1 - ya)/(log(10)*((x1 - xa)^2 + (y1 - ya)^2)) + 10*n*(yi - ya)/(log(10)*((xi - xa)^2 + (yi - ya)^2))
1181 // = 10*n*(-(y1 - ya)/(log(10)*d1a^2) + (xi - xa)/(log(10)*dia^2))
1182
1183 // for other radio source pb=(xb,yb)
1184 // diff(Prdiff1a)/diff(xb) = diff(Prdiff1a)/diff(yb) = 0
1185
1186 // For 3D:
1187 // -------
1188
1189 // Prdiff1a = Pr(pi) - Pr(p1) = 5*n*log(d1a^2) - 5*n*log(dia^2) =
1190 // = 5*n*log((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2) - 5*n*log((xi - xa)^2 + (yi - ya)^2 + (zi - za)^2)
1191
1192 // derivatives respect parameters being estimated (xi,yi,zi,xa,ya,za...,xM,yM,zM)
1193 // for unknown point pi = (xi, yi,zi)
1194 // diff(Prdiff1a)/diff(xi) = -5*n/(log(10)*((xi - xa)^2 + (yi - ya)^2 + (zi - za)^2))*2*(xi - xa)
1195 // = -10*n*(xi - xa)/(log(10)*((xi - xa)^2 + (yi - ya)^2) + (zi - za)^2))
1196 // = -10*n*(xi - xa)/(log(10)*dia^2)
1197
1198 // diff(Prdiff1a)/diff(yi) = -5*n/(log(10)*((xi - xa)^2 + (yi - ya)^2 + (zi - za)^2))*2*(yi - ya)
1199 // = -10*n*(yi - ya)/(log(10)*((xi - xa)^2 + (yi - ya)^2 + (zi - za)^2))
1200 // = -10*n*(yi - ya)/(log(10)*dia^2)
1201
1202 // diff(Prdiff1a)/diff(zi) = -5*n/(log(10)*((xi - xa)^2 + (yi - ya)^2 + (zi - za)^2))*2*(zi - za)
1203 // = -10*n*(zi - za)/(log(10)*((xi - xa)^2 + (yi - ya)^2 + (zi - za)^2))
1204 // = -10*n*(zi - za)/(log(10)*dia^2)
1205
1206 // for same radio source pa=(xa,ya)
1207 // diff(Prdiff1a)/diff(xa) = 5*n/(log(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2))*-2*(x1 - xa) -5*n/(log(10)*((xi - xa)^2 + (yi - ya)^2 + (zi - za)^2))*-2*(xi - xa) =
1208 // = -10*n*(x1 - xa)/(log(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)) + 10*n*(xi - xa)/(log(10)*((xi - xa)^2 + (yi - ya)^2 + (zi - za)^2)) =
1209 // = 10*n*(-(x1 -xa)/(log(10)*d1a^2) + (xi - xa)/(log(10)*dia^2))
1210
1211 // diff(Prdiff1a)/diff(ya) = 5*n/(log(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2))*-2*(y1 - ya) -5*n/(log(10)*((xi - xa)^2 + (yi - ya)^2 + (zi - za)^2))*-2*(yi - ya) =
1212 // = -10*n*(y1 - ya)/(log(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)) + 10*n*(yi - ya)/(log(10)*((xi - xa)^2 + (yi - ya)^2 + (zi - za)^2)) =
1213 // = 10*n(-(y1 - ya)/(log(10)*d1a^2) + (xi - xa)/(log(10)*dia^2))
1214
1215 // diff(Prdiff1a)/diff(za) = 5*n/(log(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2))*-2*(z1 - za) -5*n/(log(10)*((xi - xa)^2 + (yi - ya)^2 + (zi - za)^2))*-2*(zi - za) =
1216 // = -10*n*(z1 - za)/(log(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)) + 10*n*(zi - za)/(log(10)*((xi - xa)^2 + (yi - ya)^2 + (zi - za)^2)) =
1217 // = 10*n(-(z1 - za)/(log(10)*d1a^2) + (zi - za)/(log(10)*dia^2))
1218
1219 // for other radio source pb=(xb,yb)
1220 // diff(Prdiff1a)/diff(xb) = diff(Prdiff1a)/diff(yb) = 0
1221
1222 final var dims = NonLinearFingerprintPositionAndRadioSourceEstimator.this.getNumberOfDimensions();
1223
1224 // path loss exponent
1225 final var n = point[0];
1226
1227 final var ln10 = Math.log(10.0);
1228
1229 final var sourceIndex = allSourcesIndices.get(i);
1230 final var start = dims * (1 + sourceIndex);
1231
1232 // d1a^2, d2a^2, ...
1233 var distanceFingerprint2 = 0.0;
1234
1235 // dia^2, dib^2, ...
1236 var distancePoint2 = 0.0;
1237 for (var j = 0; j < dims; j++) {
1238 // fingerprint coordinate p1=(x1,y1,z1), ...
1239 final var fingerprintCoord = point[1 + j];
1240
1241 // unknown point "pi" coordinate
1242 final var pointCoord = params[j];
1243
1244 // radio source coordinate pa=(xa,ya,za), ...
1245 final var sourceCoord = params[start + j];
1246
1247 // x1 - xa, y1 - ya, ...
1248 final var diffFingerprint = fingerprintCoord - sourceCoord;
1249
1250 // xi - xa, yi - ya, ...
1251 final var diffPoint = pointCoord - sourceCoord;
1252
1253 final var diffFingerprint2 = diffFingerprint * diffFingerprint;
1254 final var diffPoint2 = diffPoint * diffPoint;
1255
1256 distanceFingerprint2 += diffFingerprint2;
1257 distancePoint2 += diffPoint2;
1258 }
1259
1260 distanceFingerprint2 = Math.max(distanceFingerprint2, TINY);
1261 distancePoint2 = Math.max(distancePoint2, TINY);
1262
1263 final var result = 5 * n * (Math.log10(distanceFingerprint2) - Math.log10(distancePoint2));
1264
1265
1266 // we clear derivatives array to ensure that derivatives respect other
1267 // radio sources are zero
1268 Arrays.fill(derivatives, 0.0);
1269
1270 for (var j = 0; j < dims; j++) {
1271 // fingerprint coordinate p1=(x1,y1,z1), ...
1272 final var fingerprintCoord = point[1 + j];
1273
1274 // unknown point "pi" coordinate
1275 final var pointCoord = params[j];
1276
1277 // radio source coordinate pa=(xa,ya,za), ...
1278 final var sourceCoord = params[start + j];
1279
1280 // x1 - xa, y1 - ya, ...
1281 final var diffFingerprint = fingerprintCoord - sourceCoord;
1282
1283 // xi - xa, yi - ya, ...
1284 final var diffPoint = pointCoord - sourceCoord;
1285
1286 // Example: diff(Prdiff1a)/diff(xi) = -10*n*(xi - xa)/(log(10)*dia^2)
1287 final var derivativePointCoord = -10.0 * n * diffPoint / (ln10 * distancePoint2);
1288
1289 // Example: diff(Prdiff1a)/diff(xa) = 10*n*(-(x1 - xa)/(log(10)*d1a^2) + (xi - xa)/(log(10)*dia^2)) =
1290 // -10*n*(x1 - xa)/(log(10)*d1a^2) - diff(Prdiff1a)/diff(xi)
1291 final var derivativeSameRadioSourceCoord =
1292 -10.0 * n * diffFingerprint / (ln10 * distanceFingerprint2) - derivativePointCoord;
1293
1294 // derivatives respect point pi = (xi, yi, zi)
1295 derivatives[j] = derivativePointCoord;
1296
1297 // derivatives respect same radio source pa = (xa, ya, za)
1298 derivatives[dims * (1 + sourceIndex) + j] = derivativeSameRadioSourceCoord;
1299 }
1300
1301 return result;
1302 }
1303 });
1304
1305 try {
1306 // In 2D we know that for fingerprint "1" and radio source "a":
1307 // Prdiff1a = Pr(pi) - Pr(p1) = 5*n*log(d1a^2) - 5*n*log(dia^2) =
1308 // = 5*n*log((x1 - xa)^2 + (y1 - ya)^2) - 5*n*log((xi - xa)^2 + (yi - ya)^2)
1309
1310 // Therefore x must have 1 + dims columns (for path-loss n and fingerprint position (x1,y1)
1311
1312 final var x = new Matrix(totalReadings, n);
1313 final var y = new double[totalReadings];
1314 final var standardDeviations = new double[totalReadings];
1315 for (var i = 0; i < totalReadings; i++) {
1316 // path loss exponent
1317 x.setElementAt(i, 0, allPathLossExponents.get(i));
1318
1319 final var fingerprintPosition = allFingerprintPositions.get(i);
1320 var col = 1;
1321 for (var j = 0; j < dims; j++) {
1322 x.setElementAt(i, col, fingerprintPosition.getInhomogeneousCoordinate(j));
1323 col++;
1324 }
1325
1326 y[i] = allPowerDiffs.get(i);
1327
1328 standardDeviations[i] = allStandardDeviations.get(i);
1329 }
1330
1331 fitter.setInputData(x, y, standardDeviations);
1332
1333 return allSourcesToBeEstimated;
1334 } catch (final AlgebraException e) {
1335 throw new FittingException(e);
1336 }
1337 }
1338
1339 /**
1340 * Gets the total number of readings associated to provided radio source.
1341 * This method uses only current nearest fingerprints.
1342 *
1343 * @param source radio source to be checked
1344 * @param centroid centroid to be computed.
1345 * @param fingerprints fingerprints where search is made.
1346 * @return total number of readings associated to provided radio source.
1347 */
1348 private int totalReadingsForSource(
1349 final RadioSource source,
1350 final List<RssiFingerprintLocated<RadioSource, RssiReading<RadioSource>, P>> fingerprints,
1351 final P centroid) {
1352 if (source == null) {
1353 return 0;
1354 }
1355
1356 final var dims = getNumberOfDimensions();
1357
1358 var result = 0;
1359 final var centroidCoords = centroid != null ? new double[dims] : null;
1360
1361 for (final var fingerprint : fingerprints) {
1362 final var readings = fingerprint.getReadings();
1363 if (readings == null) {
1364 continue;
1365 }
1366
1367 final var fingerprintPosition = fingerprint.getPosition();
1368
1369 for (final var reading : readings) {
1370 final var readingSource = reading.getSource();
1371 if (readingSource != null && readingSource.equals(source)) {
1372 result++;
1373
1374 if (centroid != null) {
1375 for (var i = 0; i < dims; i++) {
1376 final var coord = fingerprintPosition.getInhomogeneousCoordinate(i);
1377 centroidCoords[i] += coord;
1378 }
1379 }
1380 }
1381 }
1382 }
1383
1384 if (centroid != null && result > 0) {
1385 for (var i = 0; i < dims; i++) {
1386 centroidCoords[i] /= result;
1387 centroid.setInhomogeneousCoordinate(i, centroidCoords[i]);
1388 }
1389 }
1390
1391 return result;
1392 }
1393 }