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.Matrix;
19 import com.irurueta.geometry.Point2D;
20 import com.irurueta.navigation.indoor.IndoorException;
21 import com.irurueta.navigation.indoor.RadioSource;
22 import com.irurueta.navigation.indoor.RadioSourceLocated;
23 import com.irurueta.navigation.indoor.RssiFingerprint;
24 import com.irurueta.navigation.indoor.RssiFingerprintLocated;
25 import com.irurueta.navigation.indoor.RssiReading;
26 import com.irurueta.navigation.indoor.Utils;
27
28 import java.util.List;
29
30 /**
31 * 2D position estimator based on located fingerprints containing only RSSI readings and
32 * having as well prior knowledge of the location of radio sources associated to those
33 * readings.
34 * This implementation uses a third-order Taylor approximation over provided located
35 * fingerprints to determine an approximate position for a non-located fingerprint using
36 * a non-linear solving algorithm.
37 * An initial position can be provided as a starting point to solve the position,
38 * otherwise the average point of selected nearest fingerprints is used as a starting
39 * point.
40 */
41 public class ThirdOrderNonLinearFingerprintPositionEstimator2D extends NonLinearFingerprintPositionEstimator2D {
42
43 /**
44 * Constructor.
45 */
46 public ThirdOrderNonLinearFingerprintPositionEstimator2D() {
47 }
48
49 /**
50 * Constructor.
51 *
52 * @param listener listener in charge of handling events.
53 */
54 public ThirdOrderNonLinearFingerprintPositionEstimator2D(
55 final FingerprintPositionEstimatorListener<Point2D> listener) {
56 super(listener);
57 }
58
59 /**
60 * Constructor.
61 *
62 * @param locatedFingerprints located fingerprints containing RSSI readings.
63 * @param fingerprint fingerprint containing readings at an unknown location
64 * for provided located fingerprints.
65 * @param sources located radio sources.
66 * @throws IllegalArgumentException if provided non located fingerprint is null,
67 * located fingerprints value is null or there are not enough fingerprints or
68 * readings within provided fingerprints (for 2D position estimation at least 2
69 * located total readings are required among all fingerprints, for example 2
70 * readings are required in a single fingerprint, or at least 2 fingerprints at
71 * different locations containing a single reading are required).
72 */
73 public ThirdOrderNonLinearFingerprintPositionEstimator2D(
74 final List<? extends RssiFingerprintLocated<? extends RadioSource,
75 ? extends RssiReading<? extends RadioSource>, Point2D>> locatedFingerprints,
76 final RssiFingerprint<? extends RadioSource,
77 ? extends RssiReading<? extends RadioSource>> fingerprint,
78 final List<? extends RadioSourceLocated<Point2D>> sources) {
79 super(locatedFingerprints, fingerprint, sources);
80 }
81
82 /**
83 * Constructor.
84 *
85 * @param locatedFingerprints located fingerprints containing RSSI readings.
86 * @param fingerprint fingerprint containing readings at an unknown location
87 * for provided located fingerprints.
88 * @param sources located radio sources.
89 * @param listener listener in charge of handling events.
90 * @throws IllegalArgumentException if provided non located fingerprint is null,
91 * located fingerprints value is null or there are not enough fingerprints or
92 * readings within provided fingerprints (for 2D position estimation at least 2
93 * located total readings are required among all fingerprints, for example 2
94 * readings are required in a single fingerprint, or at least 2 fingerprints at
95 * different locations containing a single reading are required).
96 */
97 public ThirdOrderNonLinearFingerprintPositionEstimator2D(
98 final List<? extends RssiFingerprintLocated<? extends RadioSource,
99 ? extends RssiReading<? extends RadioSource>, Point2D>> locatedFingerprints,
100 final RssiFingerprint<? extends RadioSource,
101 ? extends RssiReading<? extends RadioSource>> fingerprint,
102 final List<? extends RadioSourceLocated<Point2D>> sources,
103 final FingerprintPositionEstimatorListener<Point2D> listener) {
104 super(locatedFingerprints, fingerprint, sources, listener);
105 }
106
107 /**
108 * Constructor.
109 *
110 * @param locatedFingerprints located fingerprints containing RSSI readings.
111 * @param fingerprint fingerprint containing readings at an unknown location
112 * for provided located fingerprints.
113 * @param sources located radio sources.
114 * @param initialPosition initial position to start the solving algorithm or null.
115 * @throws IllegalArgumentException if provided non located fingerprint is null,
116 * located fingerprints value is null or there are not enough fingerprints or
117 * readings within provided fingerprints (for 2D position estimation at least 2
118 * located total readings are required among all fingerprints, for example 2
119 * readings are required in a single fingerprint, or at least 2 fingerprints at
120 * different locations containing a single reading are required).
121 */
122 public ThirdOrderNonLinearFingerprintPositionEstimator2D(
123 final List<? extends RssiFingerprintLocated<? extends RadioSource,
124 ? extends RssiReading<? extends RadioSource>, Point2D>> locatedFingerprints,
125 final RssiFingerprint<? extends RadioSource,
126 ? extends RssiReading<? extends RadioSource>> fingerprint,
127 final List<? extends RadioSourceLocated<Point2D>> sources, Point2D initialPosition) {
128 super(locatedFingerprints, fingerprint, sources, initialPosition);
129 }
130
131 /**
132 * Constructor.
133 *
134 * @param locatedFingerprints located fingerprints containing RSSI readings.
135 * @param fingerprint fingerprint containing readings at an unknown location
136 * for provided located fingerprints.
137 * @param sources located radio sources.
138 * @param initialPosition initial position to start the solving algorithm or null.
139 * @param listener listener in charge of handling events.
140 * @throws IllegalArgumentException if provided non located fingerprint is null,
141 * located fingerprints value is null or there are not enough fingerprints or
142 * readings within provided fingerprints (for 2D position estimation at least 2
143 * located total readings are required among all fingerprints, for example 2
144 * readings are required in a single fingerprint, or at least 2 fingerprints at
145 * different locations containing a single reading are required).
146 */
147 public ThirdOrderNonLinearFingerprintPositionEstimator2D(
148 final List<? extends RssiFingerprintLocated<? extends RadioSource,
149 ? extends RssiReading<? extends RadioSource>, Point2D>> locatedFingerprints,
150 final RssiFingerprint<? extends RadioSource,
151 ? extends RssiReading<? extends RadioSource>> fingerprint,
152 final List<? extends RadioSourceLocated<Point2D>> sources, Point2D initialPosition,
153 final FingerprintPositionEstimatorListener<Point2D> listener) {
154 super(locatedFingerprints, fingerprint, sources, initialPosition, listener);
155 }
156
157 /**
158 * Gets type of position estimator.
159 *
160 * @return type of position estimator.
161 */
162 @Override
163 public NonLinearFingerprintPositionEstimatorType getType() {
164 return NonLinearFingerprintPositionEstimatorType.THIRD_ORDER;
165 }
166
167 /**
168 * Evaluates a non-linear multi dimension function at provided point using
169 * provided parameters and returns its evaluation and derivatives of the
170 * function respect the function parameters.
171 *
172 * @param i number of sample being evaluated.
173 * @param point point where function will be evaluated.
174 * @param params initial parameters estimation to be tried. These will
175 * change as the Levenberg-Marquardt algorithm iterates to the best solution.
176 * These are used as input parameters along with point to evaluate function.
177 * @param derivatives partial derivatives of the function respect to each
178 * provided parameter.
179 * @return function evaluation at provided point.
180 */
181 @Override
182 @SuppressWarnings("Duplicates")
183 protected double evaluate(
184 final int i, final double[] point, final double[] params, final double[] derivatives) {
185 // Demonstration in 2D:
186 // --------------------
187 // Taylor series expansion can be expressed as:
188 // f(x) = f(a) + 1/1!*f'(a)*(x - a) + 1/2!*f''(a)*(x - a)^2 + 1/3!*f'''(a)*(x - a)^3 ...
189
190 // where f'(x) is the derivative of f respect x, which can also be expressed as:
191 // f'(x) = diff(f(x))/diff(x)
192
193 // and f'(a) is the derivative of f respect x evaluated at "a", which can be expressed
194 // as f'(a) = diff(f(a))/diff(x)
195
196 // consequently f''(a) is the second derivative respect x evaluated at "a", which can
197 // be expressed as:
198 // f''(x) = diff(f(x))/diff(x^2)
199
200 // and:
201 // f''(a) = diff(f(a))/diff(x^2)
202
203 // and finally f'''(a) is the third derivative respect x evaluated at "a", which can
204 // be expressed as:
205 // f'''(x) = diff(f(x))/diff(x^3)
206
207 // and:
208 // f'''(a) = diff(f(a))/diff(x^3)
209
210 // Received power expressed in dBm is:
211 // k = (c/(4*pi*f))
212 // Pr = Pte*k^n / d^n
213
214 // where c is the speed of light, pi is 3.14159..., f is the frequency of the radio source,
215 // Pte is the equivalent transmitted power by the radio source, n is the path-loss exponent
216 // (typically 2.0), and d is the distance from a point to the location of the radio source.
217
218 // Hence:
219 // Pr(dBm) = 10*log(Pte*k^n/d^n) = 10*n*log(k) + 10*log(Pte) - 10*n*log(d) =
220 // 10*n*log(k) + 10*log(Pte) - 5*n*log(d^2)
221
222 // The former 2 terms are constant, and only the last term depends on distance
223
224 // Hence, assuming the constant K = 10*n*log(k) + Pte(dBm), where Pte(dBm) = 10*log(Pte),
225 // assuming that transmitted power by the radio source Pte is known (so that K is also known),
226 // and assuming that the location of the radio source is known, and it is located at pa = (xa, ya)
227 // so that d^2 = (x - xa)^2 + (y - ya)^2 then the received power at an unknown point pi = (xi, yi) is:
228
229 // Pr(pi) = Pr(xi,yi) = K - 5*n*log(d^2) = K - 5*n*log((xi - xa)^2 + (yi - ya)^2)
230
231 // Suppose that received power at point p1=(x1,y1) is known on a located fingerprint
232 // containing readings Pr(p1).
233
234 // Then, for an unknown point pi=(xi,yi) close to fingerprint 1 located at p1 where we
235 // have measured received power Pr(pi), we can get the following third-order Taylor
236 // approximation:
237
238 // Pr(pi = (xi,yi)) = Pr(p1) +
239 // diff(Pr(p1))/diff(x)*(xi - x1) +
240 // diff(Pr(p1))/diff(y)*(yi - y1) +
241 // 1/2*(diff(Pr(p1))/diff(x^2)*(xi - x1)^2 +
242 // diff(Pr(p1))/diff(y^2)*(yi - y1)^2 +
243 // 2*diff(Pr(p1))/diff(x*y)*(xi - x1)*(yi - y1)) +
244 // 1/6*(diff(Pr(p1))/diff(x^3)*(xi - x1)^3 +
245 // diff(Pr(p1))/diff(y^3)*(yi - y1)^3 +
246 // 3*diff(Pr(p1))/diff(x^2*y)*(xi - x1)^2*(yi - y1) +
247 // 3*diff(Pr(p1))/diff(x*y^2)*(xi - x1)*(yi - y1)^2
248
249 // where the first order derivatives of Pr(p = (x,y)) are:
250 // diff(Pr(x,y))/diff(x) = -5*n/(ln(10)*((x - xa)^2 + (y - ya)^2)*2*(x - xa)
251 // diff(Pr(x,y))/diff(x) = -10*n*(x - xa)/(ln(10)*((x - xa)^2 + (y - ya)^2))
252
253 // diff(Pr(x,y))/diff(y) = -5*n/(ln(10)*((x - xa)^2 + (y - ya)^2)*2*(y - ya)
254 // diff(Pr(x,y))/diff(y) = -10*n*(y - ya)/(ln(10)*((x - xa)^2 + (y - ya)^2))
255
256 // If we evaluate first order derivatives at p1 = (x1,y1), we get:
257 // diff(Pr(p1))/diff(x) = -10*n*(x1 - xa)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2))
258 // diff(Pr(p1))/diff(y) = -10*n*(y1 - ya)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2))
259
260 // where square distance from fingerprint 1 to radio source a can be expressed as:
261 // d1a^2 = (x1 - xa)^2 + (y1 - ya)^2
262
263 // where both the fingerprint and radio source positions are known, and hence d1a is known.
264
265 // Then first order derivatives can be expressed as:
266 // diff(Pr(p1))/diff(x) = -10*n*(x1 - xa)/(ln(10)*d1a^2)
267 // diff(Pr(p1))/diff(y) = -10*n*(y1 - ya)/(ln(10)*d1a^2)
268
269 // To obtain second order derivatives we take into account that:
270 // (f(x)/g(x))' = (f'(x)*g(x) - f(x)*g'(x))/g(x)^2
271
272 // hence, second order derivatives of Pr(p = (x,y)) are:
273 // diff(Pr(x,y))/diff(x^2) = -10*n/ln(10)*(1*((x - xa)^2 + (y - ya)^2) - (x - xa)*2*(x - xa)) / ((x - xa)^2 + (y - ya)^2)^2
274 // diff(Pr(x,y))/diff(x^2) = -10*n*((y - ya)^2 - (x - xa)^2)/(ln(10)*((x - xa)^2 + (y - ya)^2)^2)
275
276 // diff(Pr(x,y))/diff(y^2) = -10*n/ln(10)*(1*((x - xa)^2 + (y - ya)^2) - (y - ya)*2*(y - ya)) / ((x - xa)^2 + (y - ya)^2)^2
277 // diff(Pr(x,y))/diff(y^2) = -10*n*((x - xa)^2 - (y - ya)^2)/(ln(10)*((x - xa)^2 + (y - ya)^2)^2)
278
279 // diff(Pr(x,y))/diff(x*y) = -10*n/ln(10)*(0*((x - xa)^2 + (y - ya)^2) - (x - xa)*2*(y - ya))/((x - xa)^2 + (y - ya)^2)^2
280 // diff(Pr(x,y))/diff(x*y) = 20*n*((x - xa)*(y - ya))/(ln(10)*((x - xa)^2 + (y - ya)^2)^2)
281
282 // If we evaluate second order derivatives at p1 = (x1,y1), we get:
283 // diff(Pr(p1))/diff(x^2) = -10*n*((y1 - ya)^2 - (x1 - xa)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2)^2)
284 // diff(Pr(p1))/diff(y^2) = -10*n*((x1 - xa)^2 - (y1 - ya)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2)^2)
285 // diff(Pr(p1))/diff(x*y) = 20*n*(x1 - xa)*(y1 - ya)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2)^2)
286
287 // and expressing the second order derivatives in terms of distance between
288 // fingerprint 1 and radio source a d1a, we get:
289 // diff(Pr(p1))/diff(x^2) = -10*n*((y1 - ya)^2 - (x1 - xa)^2)/(ln(10)*d1a^4)
290 // diff(Pr(p1))/diff(y^2) = -10*n*((x1 - xa)^2 - (y1 - ya)^2)/(ln(10)*d1a^4)
291 // diff(Pr(p1))/diff(x*y) = 20*n*(x1 - xa)*(y1 - ya)/(ln(10)*d1a^4)
292
293 // Finally, third order derivatives of Pr(p = (x,y)) are:
294 // diff(Pr(x,y))/diff(x^3) = -10*n/ln(10)*(-2*(x - xa)*((x - xa)^2 + (y - ya)^2)^2 - ((y - ya)^2 - (x - xa)^2)*2*((x - xa)^2 + (y - ya)^2)*2*(x - xa))/((x - xa)^2 + (y - ya)^2)^4
295 // diff(Pr(x,y))/diff(y^3) = -10*n/ln(10)*(-2*(y - ya)*((x - xa)^2 + (y - ya)^2)^2 - ((x - xa)^2 - (y - ya)^2)*2*((x - xa)^2 + (y - ya)^2)*2*(y - ya))/((x - xa)^2 + (y - ya)^2)^4
296 // diff(Pr(x,y)/diff(x^2*y) = -10*n/ln(10)*(2*(y - ya)*((x - xa)^2 + (y - ya)^2)^2 - ((y - ya)^2 - (x - xa)^2)*2*((x - xa)^2 + (y - ya)^2)*2*(y - ya))/((x - xa)^2 + (y - ya)^2)^4
297 // diff(Pr(x,y)/diff(x*y^2) = -10*n/ln(10)*(2*(x - xa)*((x - xa)^2 + (y - ya)^2)^2 - ((x - xa)^2 - (y - ya)^2)*2*((x - xa)^2 + (y - ya)^2)*2*(x - xa))/((x - xa)^2 + (y - ya)^2)^4
298
299 // evaluating at p1 = (x1, y1), we get:
300 // diff(Pr(p1))/diff(x^3) = -10*n/ln(10)*(-2*(x1 - xa)*((x1 - xa)^2 + (y1 - ya)^2)^2 - ((y1 - ya)^2 - (x1 - xa)^2)*2*((x1 - xa)^2 + (y1 - ya)^2)*2*(x1 - xa))/((x1 - xa)^2 + (y1 - ya)^2)^4
301 // diff(Pr(p1))/diff(y^3) = -10*n/ln(10)*(-2*(y1 - ya)*((x1 - xa)^2 + (y1 - ya)^2)^2 - ((x1 - xa)^2 - (y1 - ya)^2)*2*((x1 - xa)^2 + (y1 - ya)^2)*2*(y1 - ya))/((x1 - xa)^2 + (y1 - ya)^2)^4
302 // diff(Pr(p1)/diff(x^2*y) = -10*n/ln(10)*(2*(y1 - ya)*((x1 - xa)^2 + (y1 - ya)^2)^2 - ((y1 - ya)^2 - (x1 - xa)^2)*2*((x1 - xa)^2 + (y1 - ya)^2)*2*(y1 - ya))/((x1 - xa)^2 + (y1 - ya)^2)^4
303 // diff(Pr(p1)/diff(x*y^2) = -10*n/ln(10)*(2*(x1 - xa)*((x1 - xa)^2 + (y1 - ya)^2)^2 - ((x1 - xa)^2 - (y1 - ya)^2)*2*((x1 - xa)^2 + (y1 - ya)^2)*2*(x1 - xa))/((x1 - xa)^2 + (y1 - ya)^2)^4
304
305 // and substituting the distance between fingerprint and radio source d1a, we get:
306 // diff(Pr(p1))/diff(x^3) = -10*n/ln(10)*(-2*(x1 - xa)*dia^4 - ((y1 - ya)^2 - (x1 - xa)^2)*4*d1a^2*(x1 - xa))/d1a^8
307 // diff(Pr(p1))/diff(y^3) = -10*n/ln(10)*(-2*(y1 - ya)*d1a^4 - ((x1 - xa)^2 - (y1 - ya)^2)*4*d1a^2*(y1 - ya))/d1a^8
308 // diff(Pr(p1)/diff(x^2*y) = -10*n/ln(10)*(2*(y1 - ya)*d1a^4 - ((y1 - ya)^2 - (x1 - xa)^2)*4*d1a^2*(y1 - ya))/d1a^8
309 // diff(Pr(p1)/diff(x*y^2) = -10*n/ln(10)*(2*(x1 - xa)*d1a^4 - ((x1 - xa)^2 - (y1 - ya)^2)*4*d1a^2*(x1 - xa))/d1a^8
310
311
312 // Hence, the third order Taylor expansion can be expressed as:
313 // Pr(pi = (xi,yi)) = Pr(p1) +
314 // diff(Pr(p1))/diff(x)*(xi - x1) +
315 // diff(Pr(p1))/diff(y)*(yi - y1) +
316 // 1/2*(diff(Pr(p1))/diff(x^2)*(xi - x1)^2 +
317 // diff(Pr(p1))/diff(y^2)*(yi - y1)^2 +
318 // 2*diff(Pr(p1))/diff(x*y)*(xi - x1)*(yi - y1)) +
319 // 1/6*(diff(Pr(p1))/diff(x^3)*(xi - x1)^3 +
320 // diff(Pr(p1))/diff(y^3)*(yi - y1)^3 +
321 // 3*diff(Pr(p1))/diff(x^2*y)*(xi - x1)^2*(yi - y1) +
322 // 3*diff(Pr(p1))/diff(x*y^2)*(xi - x1)*(yi - y1)^2)
323
324 // Pr(pi) = Pr(p1)
325 // -10*n*(x1 - xa)/(ln(10)*d1a^2)*(xi - x1) +
326 // -10*n*(y1 - ya)/(ln(10)*d1a^2)*(yi - y1) +
327 // -5*n*((y1 - ya)^2 - (x1 - xa)^2)/(ln(10)*d1a^4)*(xi - x1)^2 +
328 // -5*n*((x1 - xa)^2 - (y1 - ya)^2)/(ln(10)*d1a^4)*(yi - y1)^2 +
329 // 20*n*(x1 - xa)*(y1 - ya)/(ln(10)*d1a^4)*(xi - x1)*(yi - y1) +
330 // -10/6*n/ln(10)*(-2*(x1 - xa)*dia^4 - ((y1 - ya)^2 - (x1 - xa)^2)*4*d1a^2*(x1 - xa))/d1a^8*(xi - x1)^3 +
331 // -10/6*n/ln(10)*(-2*(y1 - ya)*d1a^4 - ((x1 - xa)^2 - (y1 - ya)^2)*4*d1a^2*(y1 - ya))/d1a^8*(yi - y1)^3 +
332 // -5*n/ln(10)*(2*(y1 - ya)*d1a^4 - ((y1 - ya)^2 - (x1 - xa)^2)*4*d1a^2*(y1 - ya))/d1a^8*(xi - x1)^2*(yi - y1) +
333 // -5*n/ln(10)*(2*(x1 - xa)*d1a^4 - ((x1 - xa)^2 - (y1 - ya)^2)*4*d1a^2*(x1 - xa))/d1a^8*(xi - x1)*(yi - y1)^2
334
335
336 // The equation above can be solved using a non-linear fitter such as Levenberg-Marquardt
337
338 // This method implements received power at point pi = (xi, yi) and its derivatives
339
340 final var xi = params[0];
341 final var yi = params[1];
342
343 // received power
344 final var pr = point[0];
345
346 // fingerprint coordinates
347 final var x1 = point[1];
348 final var y1 = point[2];
349
350 // radio source coordinates
351 final var xa = point[3];
352 final var ya = point[4];
353
354 // path loss exponent
355 final var n = point[5];
356
357 final var ln10 = Math.log(10.0);
358
359 final var diffXi1 = xi - x1;
360 final var diffYi1 = yi - y1;
361
362 final var diffX1a = x1 - xa;
363 final var diffY1a = y1 - ya;
364
365 final var diffXi12 = diffXi1 * diffXi1;
366 final var diffYi12 = diffYi1 * diffYi1;
367
368 final var diffXi13 = diffXi12 * diffXi1;
369 final var diffYi13 = diffYi12 * diffYi1;
370
371 final var diffX1a2 = diffX1a * diffX1a;
372 final var diffY1a2 = diffY1a * diffY1a;
373
374 final var d1a2 = diffX1a2 + diffY1a2;
375 final var d1a4 = d1a2 * d1a2;
376 final var d1a8 = d1a4 * d1a4;
377
378 final var value1 = -10.0 * n * diffX1a / (ln10 * d1a2);
379 final var value2 = -10.0 * n * diffY1a / (ln10 * d1a2);
380 final var value3 = -5.0 * n * (-diffX1a2 + diffY1a2) / (ln10 * d1a4);
381 final var value4 = -5.0 * n * (diffX1a2 - diffY1a2) / (ln10 * d1a4);
382 final var value5 = 20.0 * n * diffX1a * diffY1a / (ln10 * d1a4);
383 final var value6 = -10.0 / 6.0 * n / ln10 * (-2.0 * diffX1a * d1a4
384 - (-diffX1a2 + diffY1a2) * 4.0 * d1a2 * diffX1a) / d1a8;
385 final var value7 = -10.0 / 6.0 * n / ln10 * (-2.0 * diffY1a * d1a4
386 - (diffX1a2 - diffY1a2) * 4.0 * d1a2 * diffY1a) / d1a8;
387 final var value8 = -5.0 * n / ln10 * (2.0 * diffY1a * d1a4
388 - (-diffX1a2 + diffY1a2) * 4.0 * d1a2 * diffY1a) / d1a8;
389 final var value9 = -5.0 * n / ln10 * (2.0 * diffX1a * d1a4
390 - (diffX1a2 - diffY1a2) * 4.0 * d1a2 * diffX1a) / d1a8;
391
392 // hence:
393 // Pr(pi) = Pr(p1) +
394 // value1*(xi - x1) +
395 // value2*(yi - y1) +
396 // value3*(xi - x1)^2 +
397 // value4*(yi - y1)^2 +
398 // value5*(xi - x1)*(yi - y1) +
399 // value6*(xi - x1)^3 +
400 // value7*(yi - y1)^3 +
401 // value8*(xi - x1)^2*(yi - y1) +
402 // value9*(xi - x1)*(yi - y1)^2 +
403
404 final var result = pr
405 + value1 * diffXi1
406 + value2 * diffYi1
407 + value3 * diffXi12
408 + value4 * diffYi12
409 + value5 * diffXi1 * diffYi1
410 + value6 * diffXi13
411 + value7 * diffYi13
412 + value8 * diffXi12 * diffYi1
413 + value9 * diffXi1 * diffYi12;
414
415 // derivative respect xi
416
417 // diff(Pr(pi))/diff(xi) = value1 +
418 // 2*value3*(xi - x1) +
419 // value5*(yi - y1) +
420 // 3*value6*(xi - x1)^2 +
421 // 2*value8*(xi - x1)*(yi - y1) +
422 // value9*(yi - y1)^2
423
424 derivatives[0] = value1 + 2.0 * value3 * diffXi1 + value5 * diffYi1 + 3.0 * value6 * diffXi12
425 + 2.0 * value8 * diffXi1 * diffYi1 + value9 * diffYi12;
426
427 // derivative respect yi
428
429 // diff(Pr(pi))/diff(yi) = value2 +
430 // 2*value4*(yi - y1) +
431 // value5*(xi - x1) +
432 // 3*value7*(yi - y1)^2 +
433 // value8*(xi - x1)^2 +
434 // 2*value9*(xi - x1)*(yi - y1)
435
436 derivatives[1] = value2 + 2.0 * value4 * diffYi1 + value5 * diffXi1 + 3.0 * value7 * diffYi12
437 + value8 * diffXi12 + 2.0 * value9 * diffXi1 * diffYi1;
438
439 return result;
440 }
441
442 /**
443 * Propagates provided variances into RSSI variance of non-located fingerprint
444 * reading.
445 *
446 * @param fingerprintRssi closest located fingerprint reading RSSI expressed in dBm's.
447 * @param pathlossExponent path-loss exponent.
448 * @param fingerprintPosition position of closest fingerprint.
449 * @param radioSourcePosition radio source position associated to fingerprint reading.
450 * @param estimatedPosition position to be estimated. Usually this is equal to the
451 * initial position used by a non-linear algorithm.
452 * @param fingerprintRssiVariance variance of fingerprint RSSI or null if unknown.
453 * @param pathlossExponentVariance variance of path-loss exponent or null if unknown.
454 * @param fingerprintPositionCovariance covariance of fingerprint position or null if
455 * unknown.
456 * @param radioSourcePositionCovariance covariance of radio source position or null if
457 * unknown.
458 * @return variance of RSSI measured at non located fingerprint reading.
459 */
460 @Override
461 @SuppressWarnings("Duplicates")
462 protected Double propagateVariances(
463 final double fingerprintRssi, final double pathlossExponent,
464 final Point2D fingerprintPosition, final Point2D radioSourcePosition,
465 final Point2D estimatedPosition, final Double fingerprintRssiVariance,
466 final Double pathlossExponentVariance,
467 final Matrix fingerprintPositionCovariance,
468 final Matrix radioSourcePositionCovariance) {
469 try {
470 final var dist = Utils.propagateVariancesToRssiVarianceThirdOrderNonLinear2D(fingerprintRssi,
471 pathlossExponent, fingerprintPosition, radioSourcePosition, estimatedPosition,
472 fingerprintRssiVariance, pathlossExponentVariance, fingerprintPositionCovariance,
473 radioSourcePositionCovariance, null);
474 if (dist == null) {
475 return null;
476 }
477
478 final var covariance = dist.getCovariance();
479 if (covariance == null) {
480 return null;
481 }
482
483 return covariance.getElementAt(0, 0);
484
485 } catch (IndoorException e) {
486 return null;
487 }
488 }
489 }