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.fingerprint;
17
18 import com.irurueta.algebra.AlgebraException;
19 import com.irurueta.algebra.Matrix;
20 import com.irurueta.geometry.Point;
21 import com.irurueta.navigation.LockedException;
22 import com.irurueta.navigation.NotReadyException;
23 import com.irurueta.navigation.indoor.RadioSource;
24 import com.irurueta.navigation.indoor.RadioSourceKNearestFinder;
25 import com.irurueta.navigation.indoor.RadioSourceLocated;
26 import com.irurueta.navigation.indoor.RadioSourceNoMeanKNearestFinder;
27 import com.irurueta.navigation.indoor.RadioSourceWithPower;
28 import com.irurueta.navigation.indoor.RssiFingerprint;
29 import com.irurueta.navigation.indoor.RssiFingerprintLocated;
30 import com.irurueta.navigation.indoor.RssiReading;
31
32 import java.util.Collection;
33 import java.util.List;
34
35 /**
36 * Base class for position estimators based on located fingerprints containing only
37 * RSSI readings and having as well prior knowledge of the location of radio sources
38 * associated to those readings.
39 * This implementation uses a first-order Taylor approximation over provided located
40 * fingerprints to determine an approximate position for a non-located fingerprint and
41 * solves the problem in a linear way.
42 *
43 * @param <P> a {@link Point} type.
44 */
45 public abstract class LinearFingerprintPositionEstimator<P extends Point<P>> extends FingerprintPositionEstimator<P> {
46
47 /**
48 * Constructor.
49 */
50 protected LinearFingerprintPositionEstimator() {
51 }
52
53 /**
54 * Constructor.
55 *
56 * @param listener listener in charge of handling events.
57 */
58 protected LinearFingerprintPositionEstimator(final FingerprintPositionEstimatorListener<P> listener) {
59 super(listener);
60 }
61
62 /**
63 * Constructor.
64 *
65 * @param locatedFingerprints located fingerprints containing RSSI readings.
66 * @param fingerprint fingerprint containing readings at an unknown location
67 * for provided located fingerprints.
68 * @param sources located radio sources.
69 * @throws IllegalArgumentException if provided non located fingerprint is null,
70 * located fingerprints value is null or there are not enough fingerprints or
71 * readings within provided fingerprints (for 2D position estimation at least 2
72 * located total readings are required among all fingerprints, for example 2
73 * readings are required in a single fingerprint, or at least 2 fingerprints at
74 * different locations containing a single reading are required. For 3D position
75 * estimation 3 located total readings are required among all fingerprints).
76 */
77 protected LinearFingerprintPositionEstimator(
78 final List<? extends RssiFingerprintLocated<? extends RadioSource,
79 ? extends RssiReading<? extends RadioSource>, P>> locatedFingerprints,
80 final RssiFingerprint<? extends RadioSource,
81 ? extends RssiReading<? extends RadioSource>> fingerprint,
82 final List<? extends RadioSourceLocated<P>> sources) {
83 super(locatedFingerprints, fingerprint, sources);
84 }
85
86 /**
87 * Constructor.
88 *
89 * @param locatedFingerprints located fingerprints containing RSSI readings.
90 * @param fingerprint fingerprint containing readings at an unknown location
91 * for provided located fingerprints.
92 * @param sources located radio sources.
93 * @param listener listener in charge of handling events.
94 * @throws IllegalArgumentException if provided non located fingerprint is null,
95 * located fingerprints value is null or there are not enough fingerprints or
96 * readings within provided fingerprints (for 2D position estimation at least 2
97 * located total readings are required among all fingerprints, for example 2
98 * readings are required in a single fingerprint, or at least 2 fingerprints at
99 * different locations containing a single reading are required. For 3D position
100 * estimation 3 located total readings are required among all fingerprints).
101 */
102 protected LinearFingerprintPositionEstimator(
103 final List<? extends RssiFingerprintLocated<? extends RadioSource,
104 ? extends RssiReading<? extends RadioSource>, P>> locatedFingerprints,
105 final RssiFingerprint<? extends RadioSource,
106 ? extends RssiReading<? extends RadioSource>> fingerprint,
107 final List<? extends RadioSourceLocated<P>> sources,
108 final FingerprintPositionEstimatorListener<P> listener) {
109 super(locatedFingerprints, fingerprint, sources, listener);
110 }
111
112 /**
113 * Estimates position based on provided located radio sources and readings of such radio sources at
114 * an unknown location.
115 *
116 * @throws LockedException if estimator is locked.
117 * @throws NotReadyException if estimator is not ready.
118 * @throws FingerprintEstimationException if estimation fails for some other reason.
119 */
120 @Override
121 @SuppressWarnings("Duplicates")
122 public void estimate() throws LockedException, NotReadyException, FingerprintEstimationException {
123
124 if (!isReady()) {
125 throw new NotReadyException();
126 }
127 if (isLocked()) {
128 throw new LockedException();
129 }
130
131 try {
132 locked = true;
133
134 if (listener != null) {
135 listener.onEstimateStart(this);
136 }
137
138 RadioSourceNoMeanKNearestFinder<P, RadioSource> noMeanFinder = null;
139 RadioSourceKNearestFinder<P, RadioSource> finder = null;
140 if (useNoMeanNearestFingerprintFinder) {
141 //noinspection unchecked
142 noMeanFinder = new RadioSourceNoMeanKNearestFinder<>(
143 (Collection<RssiFingerprintLocated<RadioSource,
144 RssiReading<RadioSource>, P>>) locatedFingerprints);
145 } else {
146 //noinspection unchecked
147 finder = new RadioSourceKNearestFinder<>(
148 (Collection<RssiFingerprintLocated<RadioSource,
149 RssiReading<RadioSource>, P>>) locatedFingerprints);
150 }
151
152 estimatedPositionCoordinates = null;
153 nearestFingerprints = null;
154
155 final var dims = getNumberOfDimensions();
156 final var max = maxNearestFingerprints < 0
157 ? locatedFingerprints.size()
158 : Math.min(maxNearestFingerprints, locatedFingerprints.size());
159 for (var k = minNearestFingerprints; k <= max; k++) {
160
161 if (noMeanFinder != null) {
162 //noinspection unchecked
163 nearestFingerprints = noMeanFinder.findKNearestTo(
164 (RssiFingerprint<RadioSource, RssiReading<RadioSource>>) fingerprint, k);
165 } else {
166 //noinspection unchecked
167 nearestFingerprints = finder.findKNearestTo(
168 (RssiFingerprint<RadioSource, RssiReading<RadioSource>>) fingerprint, k);
169 }
170
171 // Demonstration in 2D:
172 // --------------------
173 // Taylor series expansion can be expressed as:
174 // f(x) = f(a) + 1/1!*f'(a)*(x - a) + 1/2!*f''(a)*(x - a)^2 + ...
175
176 // where f'(x) is the derivative of f respect x, which can also be expressed as:
177 // f'(x) = diff(f(x))/diff(x)
178
179 // and f'(a) is the derivative of f respect x evaluated at "a", which can be expressed
180 // as f'(a) = diff(f(a))/diff(x)
181
182 // consequently f''(a) is the second derivative respect x evaluated at "a", which can
183 // be expressed as:
184 // f''(x) = diff(f(x))/diff(x^2)
185
186 // and:
187 // f''(a) = diff(f(a))/diff(x^2)
188
189 // Received power expressed in dBm is:
190 // k = (c/(4*pi*f))
191 // Pr = Pte*k^n / d^n
192
193 // where c is the speed of light, pi is 3.14159..., f is the frequency of the radio source,
194 // Pte is the equivalent transmitted power by the radio source, n is the path-loss exponent
195 // (typically 2.0), and d is the distance from a point to the location of the radio source.
196
197 // Hence:
198 // Pr(dBm) = 10*log(Pte*k^n/d^n) = 10*n*log(k) + 10*log(Pte) - 10*n*log(d) =
199 // 10*n*log(k) + 10*log(Pte) - 5*n*log(d^2)
200
201 // The former 2 terms are constant, and only the last term depends on distance
202
203 // Hence, assuming the constant K = 10*n*log(k) + Pte(dBm), where Pte(dBm) = 10*log(Pte),
204 // assuming that transmitted power by the radio source Pte is known (so that K is also known),
205 // and assuming that the location of the radio source is known, and it is located at pa = (xa, ya)
206 // so that d^2 = (x - xa)^2 + (y - ya)^2 then the received power at an unknown point pi = (xi, yi) is:
207
208 // Pr(pi) = Pr(xi,yi) = K - 5*n*log(d^2) = K - 5*n*log((xi - xa)^2 + (yi - ya)^2)
209
210 // Suppose that received power at point p1=(x1,y1) is known on a located fingerprint
211 // containing readings Pr(p1).
212
213 // Then, for an unknown point pi=(xi,yi) close to fingerprint 1 located at p1 where we
214 // have measured received power Pr(pi), we can get the following second-order Taylor
215 // approximation:
216
217 // Pr(pi) ~ Pr(p1) + JPtr(p1)*(pi - p1) + 1/2*(pi - p1)^T*HPr(p1)*(pi - p1) + ...
218
219 // where JPr(p1) is the Jacobian of Pr evaluated at p1. Since Pr is a multivariate function
220 // with scalar result, the Jacobian has size 1x2 and is equal to the gradient.
221 // HPtr(p1) is the Hessian matrix evaluated at p1, which is a symmetric matrix of size 2x2,
222 // and (pi-p1)^T is the transposed vector of (pi-p1)
223
224 // Hence, the Jacobian at any point p=(x,y) is equal to:
225 // JPr(p = (x,y)) = [diff(Pr(x,y))/diff(x) diff(Pr(x,y))/diff(y)]
226
227 // And the Hessian matrix is equal to
228 // HPr(p = (x,y)) = [diff(Pr(x,y))/diff(x^2) diff(Pr(x,y))/diff(x*y)]
229 // [diff(Pr(x,y))/diff(x*y) diff(Pr(x,y))/diff(y^2)]
230
231 // Simplifying Taylor expansion to first-order terms to get a linear (but less accurate)
232 // solution, we get:
233 // Pr(pi) = Pr(p1) + JPtr(p1)*(pi - p1)
234 // Pr(pi) = Pr(p1) + diff(Pr(p1))/diff(x)*(xi - x1) + diff(Pr(p1))/diff(y)*(yi - y1)
235
236 // where the first order derivatives of Pr(p = (x,y)) are:
237 // diff(Pr(x,y))/diff(x) = -5*n/(ln(10)*((x - xa)^2 + (y - ya)^2)*2*(x - xa)
238 // diff(Pr(x,y))/diff(x) = -10*n*(x - xa)/(ln(10)*((x - xa)^2 + (y - ya)^2))
239
240 // diff(Pr(x,y))/diff(y) = -5*n/(ln(10)*((x - xa)^2 + (y - ya)^2)*2*(y - ya)
241 // diff(Pr(x,y))/diff(y) = -10*n*(y - ya)/(ln(10)*((x - xa)^2 + (y - ya)^2))
242
243 // If we evaluate derivatives at p1 = (x1,y1), we get:
244 // diff(Pr(p1))/diff(x) = -10*n*(x1 - xa)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2))
245 // diff(Pr(p1))/diff(y) = -10*n*(y1 - ya)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2))
246
247 // where square distance from fingerprint 1 to radio source a can be expressed as:
248 // d1a^2 = (x1 - xa)^2 + (y1 - ya)^2
249
250 // where both the fingerprint and radio source positions are known, and hence d1a is known.
251
252 // Then derivatives can be expressed as:
253 // diff(Pr(p1))/diff(x) = -10*n*(x1 - xa)/(ln(10)*d1a^2)
254 // diff(Pr(p1))/diff(y) = -10*n*(y1 - ya)/(ln(10)*d1a^2)
255
256 // Hence, first order Taylor expansion can be expressed as:
257 // Pr(pi) = Pr(p1) + diff(Pr(p1))/diff(x)*(xi - x1) + diff(Pr(p1))/diff(y)*(yi - y1)
258 // Pr(pi) = Pr(p1) - 10*n*(x1 - xa)/(ln(10)*d1a^2)*(xi - x1) - 10*n*(y1 - ya)/(ln(10)*d1a^2)*(yi - y1)
259
260 // where the only unknowns are xi,yi.
261
262 // Reordering expression above, we get:
263 // 10*n*(x1 - xa)/(ln(10)*d1a^2)*xi + 10*n*(y1 - ya)/(ln(10)*d1a^2)*yi = Pr(p1) - Pr(pi) + 10*n*(x1 - xa)/(ln(10)*d1a^2)*x1 + 10*n*(y1 - ya)/(ln(10)*d1a^2)*y1
264
265 // Which can be expressed in matrix form as:
266 // [10*n*(x1 - xa)/(ln(10)*d1a^2) 10*n*(y1 - ya)/(ln(10)*d1a^2)] [xi] = [Pr(p1) - Pr(pi) + 10*n*(x1 - xa)/(ln(10)*d1a^2)*x1 + 10*n*(y1 - ya)/(ln(10)*d1a^2)*y1]
267 // [yi]
268
269 // which is the equation obtained for fingerprint 1 and radio source "a".
270
271 // Having at least 2 linear independent equations for different fingerprints or radio sources allows
272 // solving unknown position pi = (xi,yi)
273 // Hence we could have either 2 or more located fingerprints with 1 radio sources, 2 or more radio
274 // sources on a single located fingerprint, or any combination resulting in enough equations
275
276
277 // Demonstration in 3D:
278 // --------------------
279 // Taylor series expansion can be expressed as:
280 // f(x) = f(a) + 1/1!*f'(a)*(x - a) + 1/2!*f''(a)*(x - a)^2 + ...
281
282 // where f'(x) is the derivative of f respect x, which can also be expressed as:
283 // f'(x) = diff(f(x))/diff(x)
284
285 // and f'(a) is the derivative of f respect x evaluated at "a", which can be expressed
286 // as f'(a) = diff(f(a))/diff(x)
287
288 // consequently f''(a) is the second derivative respect x evaluated at "a", which can
289 // be expressed as:
290 // f''(x) = diff(f(x))/diff(x^2)
291
292 // and:
293 // f''(a) = diff(f(a))/diff(x^2)
294
295 // Received power expressed in dBm is:
296 // k = (c/(4*pi*f))
297 // Pr = Pte*k^n / d^n
298
299 // where c is the speed of light, pi is 3.14159..., f is the frequency of the radio source,
300 // Pte is the equivalent transmitted power by the radio source, n is the path-loss exponent
301 // (typically 2.0), and d is the distance from a point to the location of the radio source.
302
303 // Hence:
304 // Pr(dBm) = 10*log(Pte*k^n/d^n) = 10*n*log(k) + 10*log(Pte) - 10*n*log(d) =
305 // 10*n*log(k) + 10*log(Pte) - 5*n*log(d^2)
306
307 // The former 2 terms are constant, and only the last term depends on distance
308
309 // Hence, assuming the constant K = 10*n*log(k) + Pte(dBm), where Pte(dBm) = 10*log(Pte),
310 // assuming that transmitted power by the radio source Pte is known (so that K is also known),
311 // and assuming that the location of the radio source is known, and it is located at pa = (xa, ya, za)
312 // so that d^2 = (x - xa)^2 + (y - ya)^2 + (z - za)^2 then the received power at an unknown point
313 // pi = (xi, yi, zi) is:
314
315 // Pr(pi) = Pr(xi,yi,zi) = K - 5*n*log(d^2) = K - 5*n*log((xi - xa)^2 + (yi - ya)^2 + (zi - za)^2)
316
317 // Suppose that received power at point p1=(x1,y1,z1) is known on a located fingerprint
318 // containing readings Pr(p1).
319
320 // Then, for an unknown point pi=(xi,yi,zi) close to fingerprint 1 located at p1 where we
321 // have measured received power Pr(pi), we can get the following second-order Taylor
322 // approximation:
323
324 // Pr(pi) ~ Pr(p1) + JPtr(p1)*(pi - p1) + 1/2*(pi - p1)^T*HPr(p1)*(pi - p1) + ...
325
326 // where JPr(p1) is the Jacobian of Pr evaluated at p1. Since Pr is a multivariate function
327 // with scalar result, the Jacobian has size 1x3 and is equal to the gradient.
328 // HPtr(p1) is the Hessian matrix evaluated at p1, which is a symmetric matrix of size 3x3,
329 // and (pi-p1)^T is the transposed vector of (pi-p1)
330
331 // Hence, the Jacobian at any point p=(x,y,z) is equal to:
332 // JPr(p = (x,y,z)) = [diff(Pr(x,y,z))/diff(x) diff(Pr(x,y,z))/diff(y) diff(Pr(x,y,z))/diff(z)]
333
334 // And the Hessian matrix is equal to
335 // HPr(p = (x,y,z)) = [diff(Pr(x,y,z))/diff(x^2) diff(Pr(x,y,z))/diff(x*y) diff(Pr(x,y,z))/diff(x*z)]
336 // [diff(Pr(x,y,z))/diff(x*y) diff(Pr(x,y,z))/diff(y^2) diff(Pr(x,y,z))/diff(y*z)]
337 // [diff(Pr(x,y,z))/diff(x*z) diff(Pr(x,y,z))/diff(y*z) diff(Pr(x,y,z))/diff(z^2)]
338
339 // Simplifying Taylor expansion to first-order terms to get a linear (but less accurate)
340 // solution, we get:
341 // Pr(pi) = Pr(p1) + JPtr(p1)*(pi - p1)
342 // Pr(pi) = Pr(p1) + diff(Pr(p1))/diff(x)*(xi - x1) + diff(Pr(p1))/diff(y)*(yi - y1) + diff(Pr(p1))/diff(z)*(zi - z1)
343
344 // where the first order derivatives of Pr(p = (x,y,z)) are:
345 // diff(Pr(x,y,z))/diff(x) = -5*n/(ln(10)*((x - xa)^2 + (y - ya)^2 + (z - za)^2)*2*(x - xa)
346 // diff(Pr(x,y,z))/diff(x) = -10*n*(x - xa)/(ln(10)*((x - xa)^2 + (y - ya)^2 + (z - za)^2))
347
348 // diff(Pr(x,y,z))/diff(y) = -5*n/(ln(10)*((x - xa)^2 + (y - ya)^2 + (z - za)^2)*2*(y - ya)
349 // diff(Pr(x,y,z))/diff(y) = -10*n*(y - ya)/(ln(10)*((x - xa)^2 + (y - ya)^2 + (z - za)^2))
350
351 // diff(Pr(x,y,z))/diff(z) = -5*n/(ln(10)*((x - xa)^2 + (y - ya)^2 + (z - za)^2)*2*(z - za)
352 // diff(Pr(x,y,z))/diff(z) = -10*n*(z - za)/(ln(10)*((x - xa)^2 + (y - ya)^2 + (z - za)^2))
353
354 // If we evaluate derivatives at p1 = (x1,y1,z1), we get:
355 // diff(Pr(p1))/diff(x) = -10*n*(x1 - xa)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2))
356 // diff(Pr(p1))/diff(y) = -10*n*(y1 - ya)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2))
357 // diff(Pr(p1))/diff(z) = -10*n*(z1 - za)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2))
358
359 // where square distance from fingerprint 1 to radio source a can be expressed as:
360 // d1a^2 = (x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2
361
362 // where both the fingerprint and radio source positions are known, and hence d1a is known.
363
364 // Then derivatives can be expressed as:
365 // diff(Pr(p1))/diff(x) = -10*n*(x1 - xa)/(ln(10)*d1a^2)
366 // diff(Pr(p1))/diff(y) = -10*n*(y1 - ya)/(ln(10)*d1a^2)
367 // diff(Pr(p1))/diff(z) = -10*n*(z1 - za)/(ln(10)*d1a^2)
368
369 // Hence, first order Taylor expansion can be expressed as:
370 // Pr(pi) = Pr(p1) + diff(Pr(p1))/diff(x)*(xi - x1) + diff(Pr(p1))/diff(y)*(yi - y1) + diff(Pr(p1))/diff(z)*(zi - z1)
371 // Pr(pi) = Pr(p1) - 10*n*(x1 - xa)/(ln(10)*d1a^2)*(xi - x1) - 10*n*(y1 - ya)/(ln(10)*d1a^2)*(yi - y1) - 10*n*(z1 - za)/(ln(10)*d1a^2)*(zi - z1)
372
373 // where the only unknowns are xi,yi,zi.
374
375 // Reordering expression above, we get:
376 // 10*n*(x1 - xa)/(ln(10)*d1a^2)*xi + 10*n*(y1 - ya)/(ln(10)*d1a^2)*yi + 10*n*(z1 - za)/(ln(10)*d1a^2)*zi = Pr(p1) - Pr(pi) + 10*n*(x1 - xa)/(ln(10)*d1a^2)*x1 + 10*n*(y1 - ya)/(ln(10)*d1a^2)*y1 + 10*n*(z1 - za)/(ln(10)*d1a^2)*z1
377
378 // Which can be expressed in matrix form as:
379 // [10*n*(x1 - xa)/(ln(10)*d1a^2) 10*n*(y1 - ya)/(ln(10)*d1a^2) 10*n*(z1 - za)/(ln(10)*d1a^2)] [xi] = [Pr(p1) - Pr(pi) + 10*n*(x1 - xa)/(ln(10)*d1a^2)*x1 + 10*n*(y1 - ya)/(ln(10)*d1a^2)*y1 + 10*n*(z1 - za)/(ln(10)*d1a^2)*z1]
380 // [yi]
381 // [zi]
382
383 // which is the equation obtained for fingerprint 1 and radio source "a".
384
385 // Having at least 3 linear independent equations for different fingerprints or radio sources allows
386 // solving unknown position pi = (xi,yi,zi)
387 // Hence we could have either 3 or more located fingerprints with 1 radio sources, 3 or more radio
388 // sources on a single located fingerprint, or any combination resulting in enough equations
389
390
391 // build system of equations
392 final var totalReadings = totalReadings(nearestFingerprints);
393
394 try {
395 final var ln10 = Math.log(10.0);
396 var row = 0;
397 final var a = new Matrix(totalReadings, dims);
398 final var b = new double[totalReadings];
399 for (final var locatedFingerprint : nearestFingerprints) {
400
401 final var fingerprintPosition = locatedFingerprint.getPosition();
402 final var locatedReadings = locatedFingerprint.getReadings();
403 if (locatedReadings == null) {
404 continue;
405 }
406
407 var locatedMeanRssi = 0.0;
408 var meanRssi = 0.0;
409 if (removeMeansFromFingerprintReadings) {
410 locatedMeanRssi = locatedFingerprint.getMeanRssi();
411 }
412
413 for (final var locatedReading : locatedReadings) {
414 final var source = locatedReading.getSource();
415
416 // find within the list of located sources the source of
417 // current located fingerprint reading.
418 // Radio sources are compared by their id
419 // regardless of them being located or not
420
421 //noinspection SuspiciousMethodCalls
422 final var pos = sources.indexOf(source);
423 if (pos < 0) {
424 continue;
425 }
426
427 final var locatedSource = sources.get(pos);
428 var pathLossExponent = this.pathLossExponent;
429 if (useSourcesPathLossExponentWhenAvailable
430 && locatedSource instanceof RadioSourceWithPower) {
431 pathLossExponent = ((RadioSourceWithPower) locatedSource).getPathLossExponent();
432 }
433
434 final var tmp = 10.0 * pathLossExponent / ln10;
435
436 final var sourcePosition = locatedSource.getPosition();
437 final var locatedRssi = locatedReading.getRssi();
438 final var sqrDistance = fingerprintPosition.sqrDistanceTo(sourcePosition);
439 if (removeMeansFromFingerprintReadings) {
440 meanRssi = fingerprint.getMeanRssi();
441 }
442
443 final var readings = fingerprint.getReadings();
444 for (final var reading : readings) {
445 if (reading.getSource() == null || !reading.getSource().equals(locatedSource)) {
446 continue;
447 }
448
449 // only take into account reading for matching sources on located and
450 // non-located readings
451 final var rssi = reading.getRssi();
452
453 // ideally if there was no bias between devices RSSI measures, we should compute:
454 // diffRssi = locatedRssi - rssi
455 // However, to account for possible biases, we remove mean of fingerprints from
456 // both readings (ideally both should be equal, but they will only be approximate in
457 // practice).
458 double diffRssi;
459 if (removeMeansFromFingerprintReadings) {
460 diffRssi = (locatedRssi - locatedMeanRssi) - (rssi - meanRssi);
461 } else {
462 diffRssi = locatedRssi - rssi;
463 }
464
465 b[row] = diffRssi;
466 for (var i = 0; i < dims; i++) {
467 final var fingerprintCoord = fingerprintPosition.getInhomogeneousCoordinate(i);
468 final var sourceCoord = sourcePosition.getInhomogeneousCoordinate(i);
469 final var diffCoord = fingerprintCoord - sourceCoord;
470
471 a.setElementAt(row, i, tmp * diffCoord / sqrDistance);
472
473 b[row] += tmp * diffCoord / sqrDistance * fingerprintCoord;
474 }
475 row++;
476 }
477 }
478 }
479
480 estimatedPositionCoordinates = com.irurueta.algebra.Utils.solve(a, b);
481
482 // a solution was found so we exit loop
483 break;
484 } catch (final AlgebraException e) {
485 // solution could not be found with current data
486 // Iterate to use additional nearby fingerprints
487 estimatedPositionCoordinates = null;
488 nearestFingerprints = null;
489 }
490 }
491
492 if (estimatedPositionCoordinates == null) {
493 // no solution could be found
494 throw new FingerprintEstimationException();
495 }
496
497 if (listener != null) {
498 listener.onEstimateEnd(this);
499 }
500 } finally {
501 locked = false;
502 }
503 }
504 }