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;
17
18 import com.irurueta.algebra.AlgebraException;
19 import com.irurueta.algebra.Matrix;
20 import com.irurueta.geometry.Point2D;
21 import com.irurueta.geometry.Point3D;
22 import com.irurueta.navigation.indoor.radiosource.RssiRadioSourceEstimator;
23 import com.irurueta.numerical.EvaluationException;
24 import com.irurueta.numerical.JacobianEstimator;
25 import com.irurueta.numerical.MultiVariateFunctionEvaluatorListener;
26 import com.irurueta.statistics.MultivariateNormalDist;
27 import com.irurueta.statistics.StatisticsException;
28
29 @SuppressWarnings("Duplicates")
30 public class Utils {
31
32 /**
33 * Speed of light expressed in meters per second (m/s).
34 */
35 public static final double SPEED_OF_LIGHT = RssiRadioSourceEstimator.SPEED_OF_LIGHT;
36
37 /**
38 * Prevents instantiation
39 */
40 private Utils() {
41 }
42
43 /**
44 * Converts from dBm's to linear power value expressed in mW.
45 *
46 * @param dBm value to be converted expressed in dBm's.
47 * @return converted value expressed in mW.
48 */
49 public static double dBmToPower(final double dBm) {
50 return Math.pow(10.0, dBm / 10.0);
51 }
52
53 /**
54 * Converts from mW to logarithmic power value expressed in dBm's.
55 *
56 * @param mW value to be converted expressed in mW's.
57 * @return converted value expressed in dBm's.
58 */
59 public static double powerTodBm(final double mW) {
60 return 10.0 * Math.log10(mW);
61 }
62
63 /**
64 * Propagates variance on received power measure into distance variance by considering the following formula
65 * for received power (expressed in dBm's):
66 * rxPower = pathLossExponent * kdB + txPower - 5.0 * pathLossExponent * logSqrDistance,
67 * where logSqrDistance is the logarithm in base 10 of the squared distance logSqrDistance = Math.log(d^2).
68 * Taking into account the previous formula, distance can be expressed as:
69 * d = 10.0^((pathLossExponent * kdB + txPower - rxPower)/(10.0 * pathLossExponent))
70 * where kdB is a constant having the following expression:
71 * kdB = 10.0 * log(c / (4 * pi * f)),
72 * where c is the speed of light and f is the frequency.
73 *
74 * @param txPower transmitted power expressed in dBm's.
75 * @param rxPower received power expressed in dBm's.
76 * @param pathLossExponent path loss exponent.
77 * @param frequency frequency expressed in Hz.
78 * @param rxPowerVariance received power variance.
79 * @return distance variance.
80 */
81 public static double propagatePowerVarianceToDistanceVariance(
82 final double txPower, final double rxPower, final double pathLossExponent, final double frequency,
83 final Double rxPowerVariance) {
84 if (rxPowerVariance == null) {
85 return 0.0;
86 }
87
88 final var k = SPEED_OF_LIGHT / (4.0 * Math.PI * frequency);
89 final var kdB = 10.0 * Math.log10(k);
90
91 // distance follows the following expression:
92 // d = 10.0^((pathLossExponent * kdB + txPower - rxPower)/(10.0 * pathLossExponent))
93 // where kdB is a constant having the following expression:
94 // kdB = 10.0 * log(c / (4 * pi * f)),
95 // where c is the speed of light and f is the frequency.
96
97 // hence, if the only unknown is the received power (x = rxPower), we can express the distance as:
98 // d = f(x) = 10.0^((pathLossExponent * kdB + txPower - x)/(10.0 * pathLossExponent))
99
100 // if we know the variance of received power var(x), then the variance of the distance will be:
101 // var(d) = var(f(x)) = (f'(E(x)))^2*var(x)
102 // where f'(x) is the derivative of f(x) evaluated at E(x) = rxPower
103
104 // the derivative f'(x) has the following expression:
105 // f'(x) = -ln(10)/(10.0*pathLossExponent)*10^((pathLossExponent * kdB + txPower - x)/(10.0 * pathLossExponent))
106
107 // evaluate derivative at E(x) = rxPower:
108 final var tenPathLossExponent = 10.0 * pathLossExponent;
109 final var derivativeF = -Math.log(10.0) / tenPathLossExponent * Math.pow(10.0,
110 (pathLossExponent * kdB + txPower - rxPower) / tenPathLossExponent);
111
112 return derivativeF * derivativeF * rxPowerVariance;
113 }
114
115 /**
116 * Propagates provided variances (transmitted power variance, received power variance and path-loss variance) into
117 * distance variance by considering the following formula for received power (expressed in dBm's):
118 * rxPower = pathLossExponent * kdB + txPower - 5.0 * pathLossExponent * logSqrDistance,
119 * where logSqrDistance is the logarithm in base 10 of the squared distance logSqrDistance = Math.log(d^2).
120 * Taking into account the previous formula, distance can be expressed as:
121 * d = 10.0^((pathLossExponent * kdB + txPower - rxPower)/(10.0 * pathLossExponent))
122 * where kdB is a constant having the following expression:
123 * kdB = 10.0 * log(c / (4 * pi * f)),
124 * where c is the speed of light and f is the frequency.
125 *
126 * @param txPower transmitted power expressed in dBm's.
127 * @param rxPower received power expressed in dBm's.
128 * @param pathLossExponent path loss exponent.
129 * @param frequency frequency expressed in Hz.
130 * @param txPowerVariance transmitted power variance.
131 * @param rxPowerVariance received power variance.
132 * @param pathLossExponentVariance path loss exponent variance.
133 * @return a normal distribution containing both expected distance and its variance.
134 * @throws IndoorException if something fails.
135 */
136 public static MultivariateNormalDist propagateVariancesToDistanceVariance(
137 final double txPower, final double rxPower, final double pathLossExponent, final double frequency,
138 final Double txPowerVariance, final Double rxPowerVariance, final Double pathLossExponentVariance)
139 throws IndoorException {
140 if (txPowerVariance == null && rxPowerVariance == null && pathLossExponentVariance == null) {
141 return null;
142 }
143
144 final var mean = new double[]{txPower, rxPower, pathLossExponent};
145 final var covariance = Matrix.diagonal(new double[]{
146 txPowerVariance != null ? txPowerVariance : 0.0,
147 rxPowerVariance != null ? rxPowerVariance : 0.0,
148 pathLossExponentVariance != null ? pathLossExponentVariance : 0.0
149 });
150
151 try {
152 return MultivariateNormalDist.propagate(new MultivariateNormalDist.JacobianEvaluator() {
153 @Override
154 public void evaluate(final double[] x, final double[] y, final Matrix jacobian) {
155 final var k = RssiRadioSourceEstimator.SPEED_OF_LIGHT / (4.0 * Math.PI * frequency);
156 final var kdB = 10.0 * Math.log10(k);
157
158 // received power in dBm's follows the equation:
159 // rxPower = pathLossExponent * kdB + txPower - 5.0 * pathLossExponent * logSqrDistance
160
161 // hence, distance follows the following expression:
162 // d = 10.0^((pathLossExponent * kdB + txPower - rxPower)/(10.0 * pathLossExponent))
163 // where kdB is a constant having the following expression:
164 // kdB = 10.0 * log(c / (4 * pi * f)),
165 // where c is the speed of light and f is the frequency.
166
167 final var logSqrDistance = (pathLossExponent * kdB + txPower - rxPower)
168 / (5.0 * pathLossExponent);
169
170 // where logSqrDistance = Math.log10(sqrDistance)
171 // and sqrDistance = distance * distance, hence
172 // logSqrDistance = Math.log10(distance * distance) = 2 * Math.log10(distance)
173
174 y[0] = Math.pow(10.0, logSqrDistance / 2.0);
175
176
177 // compute gradient (is a jacobian having 1 row and 3 columns)
178
179 // derivative of distance respect to transmitted power is:
180
181 // if the only unknown is the transmitted power (x = txPower), then:
182 // d = f(x) = 10.0^((pathLossExponent * kdB + x - rxPower)/(10.0 * pathLossExponent))
183 // and the derivative is
184 // f'(x) = ln(10)/(10.0 * pathLossExponent)*10.0^((pathLossExponent * kdB + x - rxPower)/(10.0 * pathLossExponent))
185 final var tenPathLossExponent = 10.0 * pathLossExponent;
186 final var tenPowered = Math.pow(10.0, (pathLossExponent * kdB + txPower - rxPower)
187 / tenPathLossExponent);
188 final var derivativeTxPower = Math.log(10.0) / tenPathLossExponent * tenPowered;
189
190 // derivative of distance respect to received power is:
191
192 // if the only unknown is the received power (x = rxPower), then:
193 // d = f(x) = 10.0^((pathLossExponent * kdB + txPower - x)/(10.0 * pathLossExponent))
194 // and the derivative is
195 // f'(x) = -ln(10)/(10.0*pathLossExponent)*10^((pathLossExponent * kdB + txPower - x)/(10.0 * pathLossExponent))
196 final var derivativeRxPower = -Math.log(10.0) / tenPathLossExponent * tenPowered;
197
198 // derivative respect to path loss exponent is:
199
200 // if the only unknown is the path loss exponent (x = pathLossExponent), then:
201 // d = f(x) = 10.0^((x * kdB + txPower - rxPower)/(10.0 * x))
202 // and the derivative is:
203 // f'(x) = ln(10) * g'(x) * 10.0^(g(x))
204 // where g(x) is:
205 // g(x) = (x * kdB + txPower - rxPower) / (10.0 * x)
206 // and the derivative of g(x) is:
207 // g'(x) = (kdB * 10.0 * x - 10.0 * (x * kdB + txPower - rxPower)) / (10.0 * x)^2
208 // Hence:
209 // f'(x) = lng(10) * (kdB * 10.0 * x - 10.0 * (x * kdB + txPower - rxPower)) / (10.0 * x)^2 * 10.0^((x * kdB + txPower - rxPower)/(10.0 * x))
210
211 final var g = (pathLossExponent * kdB + txPower - rxPower) / (10.0 * pathLossExponent);
212 final var derivativeG = (kdB * 10.0 * pathLossExponent
213 - 10.0 * (pathLossExponent * kdB + txPower - rxPower))
214 / Math.pow(10.0 * pathLossExponent, 2.0);
215
216 final var derivativePathLossExponent = Math.log(10.0) * derivativeG * Math.pow(10.0, g);
217
218 jacobian.setElementAtIndex(0, derivativeTxPower);
219 jacobian.setElementAtIndex(1, derivativeRxPower);
220 jacobian.setElementAtIndex(2, derivativePathLossExponent);
221 }
222
223 @Override
224 public int getNumberOfVariables() {
225 return 1;
226 }
227 }, mean, covariance);
228 } catch (final AlgebraException | StatisticsException e) {
229 throw new IndoorException(e);
230 }
231 }
232
233 /**
234 * Propagates provided variances (fingerprint rssi variance, path-loss exponent variance,
235 * fingerprint position covariance and radio source position covariance) into
236 * rssi variance by considering the 2D 1st order Taylor expression of received power.
237 * Notice that any unknown variance is assumed to be zero.
238 *
239 * @param fingerprintRssi closest located fingerprint reading RSSI expressed in dBm's.
240 * @param pathLossExponent path-loss exponent.
241 * @param fingerprintPosition position of closest fingerprint.
242 * @param radioSourcePosition radio source position associated to fingerprint reading.
243 * @param estimatedPosition position to be estimated. Usually this is equal to the
244 * initial position used by a non-linear algorithm.
245 * @param fingerprintRssiVariance variance of fingerprint RSSI or null if unknown.
246 * @param pathLossExponentVariance variance of path-loss exponent or null if unknown.
247 * @param fingerprintPositionCovariance covariance of fingerprint position or null if
248 * unknown.
249 * @param radioSourcePositionCovariance covariance of radio source position or null
250 * if unknown.
251 * @param estimatedPositionCovariance covariance of position to be estimated or null
252 * if unknown. (This is usually unknown).
253 * @return a normal distribution containing expected received RSSI value and its variance.
254 * @throws IndoorException if something fails.
255 */
256 public static MultivariateNormalDist propagateVariancesToRssiVarianceFirstOrderNonLinear2D(
257 final double fingerprintRssi, final double pathLossExponent, final Point2D fingerprintPosition,
258 final Point2D radioSourcePosition, final Point2D estimatedPosition, final Double fingerprintRssiVariance,
259 final Double pathLossExponentVariance, final Matrix fingerprintPositionCovariance,
260 final Matrix radioSourcePositionCovariance, final Matrix estimatedPositionCovariance)
261 throws IndoorException {
262
263 if (fingerprintPosition == null || radioSourcePosition == null || estimatedPosition == null) {
264 return null;
265 }
266
267 // 1st order Taylor expression of received power in 2D:
268 // Pr(pi) = Pr(p1)
269 // - 10*n*(x1 - xa)/(ln(10)*d1a^2)*(xi - x1)
270 // - 10*n*(y1 - ya)/(ln(10)*d1a^2)*(yi - y1)
271 // where d1a^2 = (x1 - xa)^2 + (y1 - ya)^2
272
273 final var x1 = fingerprintPosition.getInhomX();
274 final var y1 = fingerprintPosition.getInhomY();
275
276 final var xa = radioSourcePosition.getInhomX();
277 final var ya = radioSourcePosition.getInhomY();
278
279 final var xi = estimatedPosition.getInhomX();
280 final var yi = estimatedPosition.getInhomY();
281
282 final var mean = new double[]{
283 fingerprintRssi, pathLossExponent, x1, y1, xa, ya, xi, yi
284 };
285 final var covariance = Matrix.diagonal(new double[]{
286 fingerprintRssiVariance != null ? fingerprintRssiVariance : 0.0,
287 pathLossExponentVariance != null ? pathLossExponentVariance : 0.0,
288 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
289 });
290
291 if (fingerprintPositionCovariance != null
292 && fingerprintPositionCovariance.getRows() == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH
293 && fingerprintPositionCovariance.getColumns() == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH) {
294
295 covariance.setSubmatrix(2, 2, 3, 3,
296 fingerprintPositionCovariance);
297 }
298
299 if (radioSourcePositionCovariance != null
300 && radioSourcePositionCovariance.getRows() == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH
301 && radioSourcePositionCovariance.getColumns() == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH) {
302 covariance.setSubmatrix(4, 4, 5, 5,
303 radioSourcePositionCovariance);
304 }
305
306 if (estimatedPositionCovariance != null
307 && estimatedPositionCovariance.getRows() == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH
308 && estimatedPositionCovariance.getColumns() == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH) {
309 covariance.setSubmatrix(6, 6, 7, 7,
310 estimatedPositionCovariance);
311 }
312
313 try {
314 return MultivariateNormalDist.propagate(new MultivariateNormalDist.JacobianEvaluator() {
315 @Override
316 public void evaluate(final double[] x, final double[] y, final Matrix jacobian) {
317
318 // Pr(pi) = Pr(p1)
319 // - 10*n*(x1 - xa)/(ln(10)*d1a^2)*(xi - x1)
320 // - 10*n*(y1 - ya)/(ln(10)*d1a^2)*(yi - y1)
321 // where d1a^2 = (x1 - xa)^2 + (y1 - ya)^2
322
323 // Hence:
324 // Pr(pi) = Pr(p1) -10*n*((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))/
325 // (ln(10)*((x1 - xa)^2 + (y1 - ya)^2))
326
327 final var diffX1a = x1 - xa;
328 final var diffY1a = y1 - ya;
329
330 final var diffXi1 = xi - x1;
331 final var diffYi1 = yi - y1;
332
333 final var diffX1a2 = diffX1a * diffX1a;
334 final var diffY1a2 = diffY1a * diffY1a;
335
336 final var d1a2 = diffX1a2 + diffY1a2;
337 final var d1a4 = d1a2 * d1a2;
338
339 final var ln10 = Math.log(10.0);
340 final var crossDiff = diffX1a * diffXi1 + diffY1a * diffYi1;
341
342 y[0] = fingerprintRssi - 10.0 * pathLossExponent * crossDiff / (ln10 * d1a2);
343
344 // compute gradient (is a jacobian having 1 row and 8 columns)
345
346 // derivative of rssi respect to fingerprint rssi
347 final var derivativeFingerprintRssi = 1.0;
348
349 // derivative of rssi respect to path-loss exponent
350
351 // diff(Pr(pi))/diff(n) = -10*(x1 - xa)/(ln(10)*d1a^2)*(xi - x1)
352 // -10*(y1 - ya)/(ln(10)*d1a^2)*(yi - y1)
353 final var derivativePathLossExponent = -10.0 * crossDiff / (ln10 * d1a2);
354
355 // derivative of rssi respect to x1
356
357 // We have
358 // Pr(pi) = Pr(p1) -10*n*((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))/
359 // (ln(10)*((x1 - xa)^2 + (y1 - ya)^2))
360
361 // and we know that: (f(x)/g(x))' = (f'(x)*g(x) - f(x)*g'(x))/g(x)^2
362 // and also that (f(x)*g(x))' = f'(x)*g(x) + f(x)*g'(x)
363
364 // Hence
365 // diff((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))/diff(x1) =
366 // diff(x1*xi -xa*xi -x1^2 + xa*x1)/diff(x1) =
367 // diff(-x1^2 + (xi + xa)*x1 - xa*xi)/diff(x1) =
368 // -2*x1 + xi + xa
369
370 // diff(Pr(pi))/diff(x1) = -10*n/ln(10)*((-2*x1 + xi + xa)*((x1 - xa)^2 + (y1 - ya)^2) - ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))*2*(x1 - xa))/((x1 - xa)^2 + (y1 - ya)^2)^2
371 // diff(Pr(pi))/diff(x1) = -10*n/ln(10)*((-2*x1 + xi + xa)*d1a^2 - ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))*2*(x1 - xa))/d1a^4
372 final var tmpX = 2.0 * crossDiff * diffX1a;
373 final var derivativeX1 = -10.0 * pathLossExponent / ln10 * ((-2.0 * x1 + xi + xa) * d1a2
374 - tmpX) / d1a4;
375
376 // derivative of rssi respect to y1
377
378 // diff((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))/diff(y1) =
379 // diff(y1*yi -ya*yi -y1^2 + ya*y1)/diff(y1) =
380 // diff(-y1^2 + (yi + ya)*y1 - ya*yi)/diff(y1) =
381 // -2*y1 + yi + ya
382
383 // diff(Pr(pi))/diff(y1) = -10*n/ln(10)*((-2*y1 + yi + ya)*((x1 - xa)^2 + (y1 - ya)^2) - ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))*2*(y1 - ya))/((x1 - xa)^2 + (y1 - ya)^2)^2
384 // diff(Pr(pi))/diff(y1) = -10*n/ln(10)*((-2*y1 + yi + ya)*d1a^2 - ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))*2*(y1 - ya))/d1a^4
385 final var tmpY = 2.0 * crossDiff * diffY1a;
386 final var derivativeY1 = -10.0 * pathLossExponent / ln10 * ((-2.0 * y1 + yi + ya) * d1a2
387 - tmpY) / d1a4;
388
389
390 // derivative of rssi respect to xa
391
392 // diff((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))/diff(xa) =
393 // diff(x1*xi -xa*xi -x1^2 + xa*x1)/diff(xa) =
394 // x1 - xi
395
396 // diff(Pr(pi))/diff(xa) = -10*n/ln(10)*((x1 - xi)*((x1 - xa)^2 + (y1 - ya)^2) - ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))*-2*(x1 - xa))/((x1 - xa)^2 + (y1 - ya)^2)^2
397 // diff(Pr(pi))/diff(xa) = -10*n/ln(10)*(-(xi - x1)*d1a^2 + ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))*2*(x1 - xa))/d1a^4
398 final var derivativeXa = -10.0 * pathLossExponent / ln10 * (-diffXi1 * d1a2 + tmpX) / d1a4;
399
400 // derivative of rssi respect to ya
401
402 // diff((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))/diff(ya) =
403 // diff(y1*yi -y1^2 -ya*yi + ya*y1)/diff(ya) =
404 // y1 - yi
405
406 // diff(Pr(pi))/diff(ya) = -10*n/ln(10)*((y1 - yi)*((x1 - xa)^2 + (y1 - ya)^2) - ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))*-2*(y1 - ya))/((x1 - xa)^2 + (y1 - ya)^2)^2
407 // diff(Pr(pi))/diff(ya) = -10*n/ln(10)*(-(yi - y1)*d1a^2 + ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))*2*(y1 - ya))/d1a^4
408 final var derivativeYa = -10.0 * pathLossExponent / ln10 * (-diffYi1 * d1a2 + tmpY) / d1a4;
409
410 // derivative of rssi respect to xi
411
412 // Pr(pi) = Pr(p1) -10*n*((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))/
413 // (ln(10)*((x1 - xa)^2 + (y1 - ya)^2))
414
415 // diff((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))/diff(xi) =
416 // diff(x1*xi -xa*xi -x1^2 + xa*x1)/diff(xi) =
417 // x1 - xa
418
419 // diff(Pr(pi))/diff(xi) = -10*n/ln(10)*((x1 - xa)*((x1 - xa)^2 + (y1 - ya)^2) - ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))*0)/(x1 - xa)^2 + (y1 - ya)^2)^2
420 // diff(Pr(pi))/diff(xi) = -10*n/ln(10)*((x1 - xa)*d1a^2)/d1a^4
421 // diff(Pr(pi))/diff(xi) = -10*n*(x1 - xa)/(ln(10)*d1a^2)
422 final var derivativeXi = -10.0 * pathLossExponent * diffX1a / (ln10 * d1a2);
423
424 // derivative of rssi respect to yi
425
426 // Pr(pi) = Pr(p1) -10*n*((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))/
427 // (ln(10)*((x1 - xa)^2 + (y1 - ya)^2))
428
429 // diff((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))/diff(yi) =
430 // diff(y1*yi -ya*yi -y1^2 + ya*y1)/diff(yi) =
431 // y1 - ya
432
433 // diff(Pr(pi))/diff(yi) = -10*n/ln(10)*((y1 - ya)*((x1 - xa)^2 + (y1 - ya)^2) - ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))*0)/((x1 - xa)^2 + (y1 - ya)^2)^2
434 // diff(Pr(pi))/diff(yi) = -10*n/ln(10)*((y1 - ya)*d1a^2)/d1a^4
435 // diff(Pr(pi))/diff(yi) = -10*n*(y1 - ya)/(ln(10)*d1a^2)
436 final var derivativeYi = -10.0 * pathLossExponent * diffY1a / (ln10 * d1a2);
437
438 // set derivatives fingerprintRssi, pathLossExponent, x1, y1, xa, ya, xi, yi
439 jacobian.setElementAtIndex(0, derivativeFingerprintRssi);
440 jacobian.setElementAtIndex(1, derivativePathLossExponent);
441 jacobian.setElementAtIndex(2, derivativeX1);
442 jacobian.setElementAtIndex(3, derivativeY1);
443 jacobian.setElementAtIndex(4, derivativeXa);
444 jacobian.setElementAtIndex(5, derivativeYa);
445 jacobian.setElementAtIndex(6, derivativeXi);
446 jacobian.setElementAtIndex(7, derivativeYi);
447 }
448
449 @Override
450 public int getNumberOfVariables() {
451 return 1;
452 }
453 }, mean, covariance);
454 } catch (final AlgebraException | StatisticsException e) {
455 throw new IndoorException(e);
456 }
457 }
458
459 /**
460 * Propagates provided variances (fingerprint rssi variance, path-loss exponent variance,
461 * fingerprint position covariance and radio source position covariance) into
462 * rssi variance by considering the 3D 1st order Taylor expression of received power.
463 * Notice that any unknown variance is assumed to be zero.
464 *
465 * @param fingerprintRssi closest located fingerprint reading RSSI expressed in dBm's.
466 * @param pathLossExponent path-loss exponent.
467 * @param fingerprintPosition position of closest fingerprint.
468 * @param radioSourcePosition radio source position associated to fingerprint reading.
469 * @param estimatedPosition position to be estimated. Usually this is equal to the
470 * initial position used by a non-linear algorithm.
471 * @param fingerprintRssiVariance variance of fingerprint RSSI or null if unknown.
472 * @param pathLossExponentVariance variance of path-loss exponent or null if unknown.
473 * @param fingerprintPositionCovariance covariance of fingerprint position or null if
474 * unknown.
475 * @param radioSourcePositionCovariance covariance of radio source position or null
476 * if unknown.
477 * @param estimatedPositionCovariance covariance of position to be estimated or null
478 * if unknown. (This is usually unknown).
479 * @return a normal distribution containing expected received RSSI value and its variance.
480 * @throws IndoorException if something fails.
481 */
482 public static MultivariateNormalDist propagateVariancesToRssiVarianceFirstOrderNonLinear3D(
483 final double fingerprintRssi, final double pathLossExponent, final Point3D fingerprintPosition,
484 final Point3D radioSourcePosition, final Point3D estimatedPosition, final Double fingerprintRssiVariance,
485 final Double pathLossExponentVariance, final Matrix fingerprintPositionCovariance,
486 final Matrix radioSourcePositionCovariance, final Matrix estimatedPositionCovariance)
487 throws IndoorException {
488
489 if (fingerprintPosition == null || radioSourcePosition == null || estimatedPosition == null) {
490 return null;
491 }
492
493 // 1st order Taylor expression of received power in 3D:
494 // Pr(pi) = Pr(p1)
495 // - 10*n*(x1 - xa)/(ln(10)*d1a^2)*(xi - x1)
496 // - 10*n*(y1 - ya)/(ln(10)*d1a^2)*(yi - y1)
497 // - 10*n*(z1 - za)/(ln(10)*d1a^2)*(zi - z1)
498 // where d1a^2 = (x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2
499
500 final var x1 = fingerprintPosition.getInhomX();
501 final var y1 = fingerprintPosition.getInhomY();
502 final var z1 = fingerprintPosition.getInhomZ();
503
504 final var xa = radioSourcePosition.getInhomX();
505 final var ya = radioSourcePosition.getInhomY();
506 final var za = radioSourcePosition.getInhomZ();
507
508 final var xi = estimatedPosition.getInhomX();
509 final var yi = estimatedPosition.getInhomY();
510 final var zi = estimatedPosition.getInhomZ();
511
512 final var mean = new double[]{
513 fingerprintRssi, pathLossExponent, x1, y1, z1, xa, ya, za, xi, yi, zi
514 };
515 final var covariance = Matrix.diagonal(new double[]{
516 fingerprintRssiVariance != null ? fingerprintRssiVariance : 0.0,
517 pathLossExponentVariance != null ? pathLossExponentVariance : 0.0,
518 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
519 });
520
521 if (fingerprintPositionCovariance != null &&
522 fingerprintPositionCovariance.getRows() == Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH &&
523 fingerprintPositionCovariance.getColumns() == Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH) {
524
525 covariance.setSubmatrix(2, 2, 4, 4,
526 fingerprintPositionCovariance);
527 }
528
529 if (radioSourcePositionCovariance != null
530 && radioSourcePositionCovariance.getRows() == Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH
531 && radioSourcePositionCovariance.getColumns() == Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH) {
532 covariance.setSubmatrix(5, 5, 7, 7,
533 radioSourcePositionCovariance);
534 }
535
536 if (estimatedPositionCovariance != null
537 && estimatedPositionCovariance.getRows() == Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH
538 && estimatedPositionCovariance.getColumns() == Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH) {
539 covariance.setSubmatrix(8, 8, 10, 10,
540 estimatedPositionCovariance);
541 }
542
543 try {
544 return MultivariateNormalDist.propagate(new MultivariateNormalDist.JacobianEvaluator() {
545 @Override
546 public void evaluate(final double[] x, final double[] y, final Matrix jacobian) {
547
548 // Pr(pi) = Pr(p1)
549 // - 10*n*(x1 - xa)/(ln(10)*d1a^2)*(xi - x1)
550 // - 10*n*(y1 - ya)/(ln(10)*d1a^2)*(yi - y1)
551 // - 10*n*(z1 - za)/(ln(10)*d1a^2)*(zi - z1)
552 // where d1a^2 = (x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2
553
554 // Hence:
555 // Pr(pi) = Pr(p1) -10*n*((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))/
556 // (ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2))
557
558 final var diffX1a = x1 - xa;
559 final var diffY1a = y1 - ya;
560 final var diffZ1a = z1 - za;
561
562 final var diffXi1 = xi - x1;
563 final var diffYi1 = yi - y1;
564 final var diffZi1 = zi - z1;
565
566 final var diffX1a2 = diffX1a * diffX1a;
567 final var diffY1a2 = diffY1a * diffY1a;
568 final var diffZ1a2 = diffZ1a * diffZ1a;
569
570 final var d1a2 = diffX1a2 + diffY1a2 + diffZ1a2;
571 final var d1a4 = d1a2 * d1a2;
572
573 final var ln10 = Math.log(10.0);
574 final var crossDiff = diffX1a * diffXi1 + diffY1a * diffYi1 + diffZ1a * diffZi1;
575
576 y[0] = fingerprintRssi - 10.0 * pathLossExponent * crossDiff / (ln10 * d1a2);
577
578 // compute gradient (is a jacobian having 1 row and 11 columns)
579
580
581 // derivative of rssi respect to fingerprint rssi
582 final var derivativeFingerprintRssi = 1.0;
583
584 // derivative of rssi respect to path-loss exponent
585
586 // diff(Pr(pi))/diff(n) = -10*(x1 - xa)/(ln(10)*d1a^2)*(xi - x1)
587 // -10*(y1 - ya)/(ln(10)*d1a^2)*(yi - y1)
588 final var derivativePathLossExponent = -10.0 * crossDiff / (ln10 * d1a2);
589
590 // derivative of rssi respect to x1
591
592 // We have
593 // Pr(pi) = Pr(p1) -10*n*((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))/
594 // (ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2))
595
596 // and we know that: (f(x)/g(x))' = (f'(x)*g(x) - f(x)*g'(x))/g(x)^2
597 // and also that (f(x)*g(x))' = f'(x)*g(x) + f(x)*g'(x)
598
599 // Hence
600 // diff((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))/diff(x1) =
601 // diff(x1*xi -xa*xi -x1^2 + xa*x1)/diff(x1) =
602 // diff(-x1^2 + (xi + xa)*x1 - xa*xi)/diff(x1) =
603 // -2*x1 + xi + xa
604
605 // diff(Pr(pi))/diff(x1) = -10*n/ln(10)*((-2*x1 + xi + xa)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2) - ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))*2*(x1 - xa))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2
606 // diff(Pr(pi))/diff(x1) = -10*n/ln(10)*((-2*x1 + xi + xa)*d1a^2 - ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))*2*(x1 - xa))/d1a^4
607 final var tmpX = 2.0 * crossDiff * diffX1a;
608 final var derivativeX1 = -10.0 * pathLossExponent / ln10 * ((-2.0 * x1 + xi + xa) * d1a2
609 - tmpX) / d1a4;
610
611 // derivative of rssi respect to y1
612
613 // diff((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))/diff(y1) =
614 // diff(y1*yi -ya*yi -y1^2 + ya*y1)/diff(y1) =
615 // diff(-y1^2 + (yi + ya)*y1 - ya*yi)/diff(y1) =
616 // -2*y1 + yi + ya
617
618 // diff(Pr(pi))/diff(y1) = -10*n/ln(10)*((-2*y1 + yi + ya)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2) - ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))*2*(y1 - ya))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2
619 // diff(Pr(pi))/diff(y1) = -10*n/ln(10)*((-2*y1 + yi + ya)*d1a^2 - ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))*2*(y1 - ya))/d1a^4
620 final var tmpY = 2.0 * crossDiff * diffY1a;
621 final var derivativeY1 = -10.0 * pathLossExponent / ln10 * ((-2.0 * y1 + yi + ya) * d1a2
622 - tmpY) / d1a4;
623
624 // derivative of rssi respect to z1
625
626 // diff((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))/diff(z1) =
627 // diff(z1*zi -za*zi -z1^2 + za*z1)/diff(z1) =
628 // diff(-z1^2 + (zi + za)*z1 - za*zi)/diff(z1) =
629 // -2*z1 + zi + za
630
631 // diff(Pr(pi))/diff(z1) = -10*n/ln(10)*((-2*z1 + zi + za)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2) - ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))*2*(z1 - za))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2
632 // diff(Pr(pi))/diff(z1) = -10*n/ln(10)*((-2*z1 + zi + za)*d1a^2 - ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))*2*(z1 - za))/d1a^4
633 final var tmpZ = 2.0 * crossDiff * diffZ1a;
634 final var derivativeZ1 = -10.0 * pathLossExponent / ln10 * ((-2.0 * z1 + z1 + za) * d1a2
635 - tmpZ) / d1a4;
636
637 // derivative of rssi respect to xa
638
639 // diff((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))/diff(xa) =
640 // diff(x1*xi -xa*xi -x1^2 + xa*x1)/diff(xa) =
641 // x1 - xi
642
643 // diff(Pr(pi))/diff(xa) = -10*n/ln(10)*((x1 - xi)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2) - ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))*-2*(x1 - xa))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2
644 // diff(Pr(pi))/diff(xa) = -10*n/ln(10)*(-(xi - x1)*d1a^2 + ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))*2*(x1 - xa))/d1a^4
645 final var derivativeXa = -10.0 * pathLossExponent / ln10 * (-diffXi1 * d1a2 + tmpX) / d1a4;
646
647 // derivative of rssi respect to ya
648
649 // diff((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))/diff(ya) =
650 // diff(y1*yi -y1^2 -ya*yi + ya*y1)/diff(ya) =
651 // y1 - yi
652
653 // diff(Pr(pi))/diff(ya) = -10*n/ln(10)*((y1 - yi)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2) - ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))*-2*(y1 - ya))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2
654 // diff(Pr(pi))/diff(ya) = -10*n/ln(10)*(-(yi - y1)*d1a^2 + ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))*2*(y1 - ya))/d1a^4
655 final var derivativeYa = -10.0 * pathLossExponent / ln10 * (-diffYi1 * d1a2 + tmpY) / d1a4;
656
657 // derivative of rssi respect to za
658
659 // diff((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))/diff(za) =
660 // diff(z1*zi -z1^2 -za*zi + za*z1)/diff(za) =
661 // z1 - zi
662
663 // diff(Pr(pi))/diff(za) = -10*n/ln(10)*((z1 - zi)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2) - ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))*-2*(z1 - za))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2
664 // diff(Pr(pi))/diff(za) = -10*n/ln(10)*(-(zi - z1)*d1a^2 + ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))*2*(z1 - za))/d1a^4
665 final var derivativeZa = -10.0 * pathLossExponent / ln10 * (-diffZi1 * d1a2 + tmpZ) / d1a4;
666
667 // derivative of rssi respect to xi
668
669 // Pr(pi) = Pr(p1) -10*n*((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))/
670 // (ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2))
671
672 // diff((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))/diff(xi) =
673 // diff(x1*xi -xa*xi -x1^2 + xa*x1)/diff(xi) =
674 // x1 - xa
675
676 // diff(Pr(pi))/diff(xi) = -10*n/ln(10)*((x1 - xa)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2) - ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))*0)/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2
677 // diff(Pr(pi))/diff(xi) = -10*n/ln(10)*((x1 - xa)*d1a^2)/d1a^4
678 // diff(Pr(pi))/diff(xi) = -10*n*(x1 - xa)/(ln(10)*d1a^2)
679 final var derivativeXi = -10.0 * pathLossExponent * diffX1a / (ln10 * d1a2);
680
681 // derivative of rssi respect to yi
682
683 // Pr(pi) = Pr(p1) -10*n*((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))/
684 // (ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2))
685
686 // diff((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))/diff(yi) =
687 // diff(y1*yi -ya*yi -y1^2 + ya*y1)/diff(yi) =
688 // y1 - ya
689
690 // diff(Pr(pi))/diff(yi) = -10*n/ln(10)*((y1 - ya)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2) - ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))*0)/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2
691 // diff(Pr(pi))/diff(yi) = -10*n/ln(10)*((y1 - ya)*d1a^2)/d1a^4
692 // diff(Pr(pi))/diff(yi) = -10*n*(y1 - ya)/(ln(10)*d1a^2)
693 final var derivativeYi = -10.0 * pathLossExponent * diffY1a / (ln10 * d1a2);
694
695 // derivative of rssi respect to zi
696
697 // Pr(pi) = Pr(p1) -10*n*((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))/
698 // (ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2))
699
700 // diff((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))/diff(zi) =
701 // diff(z1*zi -za*zi -z1^2 + za*z1)/diff(zi) =
702 // z1 - za
703
704 // diff(Pr(pi))/diff(zi) = -10*n/ln(10)*((z1 - za)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2) - ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))*0)/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2
705 // diff(Pr(pi))/diff(zi) = -10*n/ln(10)*((z1 - za)*d1a^2)/d1a^4
706 // diff(Pr(pi))/diff(zi) = -10*n*(z1 - za)/(ln(10*d1a^2)
707 final var derivativeZi = -10.0 * pathLossExponent * diffZ1a / (ln10 * d1a2);
708
709 // set derivatives fingerprintRssi, pathLossExponent, x1, y1, z1, xa, ya, za, xi, yi, zi
710 jacobian.setElementAtIndex(0, derivativeFingerprintRssi);
711 jacobian.setElementAtIndex(1, derivativePathLossExponent);
712 jacobian.setElementAtIndex(2, derivativeX1);
713 jacobian.setElementAtIndex(3, derivativeY1);
714 jacobian.setElementAtIndex(4, derivativeZ1);
715 jacobian.setElementAtIndex(5, derivativeXa);
716 jacobian.setElementAtIndex(6, derivativeYa);
717 jacobian.setElementAtIndex(7, derivativeZa);
718 jacobian.setElementAtIndex(8, derivativeXi);
719 jacobian.setElementAtIndex(9, derivativeYi);
720 jacobian.setElementAtIndex(10, derivativeZi);
721 }
722
723 @Override
724 public int getNumberOfVariables() {
725 return 1;
726 }
727 }, mean, covariance);
728 } catch (final AlgebraException | StatisticsException e) {
729 throw new IndoorException(e);
730 }
731 }
732
733 /**
734 * Propagates provided variances (fingerprint rssi variance, path-loss exponent variance,
735 * fingerprint position covariance and radio source position covariance) into
736 * rssi variance by considering the 2D 2nd order Taylor expression of received power.
737 * Notice that any unknown variance is assumed to be zero.
738 *
739 * @param fingerprintRssi closest located fingerprint reading RSSI expressed in dBm's.
740 * @param pathLossExponent path-loss exponent.
741 * @param fingerprintPosition position of closest fingerprint.
742 * @param radioSourcePosition radio source position associated to fingerprint reading.
743 * @param estimatedPosition position to be estimated. Usually this is equal to the
744 * initial position used by a non-linear algorithm.
745 * @param fingerprintRssiVariance variance of fingerprint RSSI or null if unknown.
746 * @param pathLossExponentVariance variance of path-loss exponent or null if unknown.
747 * @param fingerprintPositionCovariance covariance of fingerprint position or null if
748 * unknown.
749 * @param radioSourcePositionCovariance covariance of radio source position or null
750 * if unknown.
751 * @param estimatedPositionCovariance covariance of position to be estimated or null
752 * if unknown. (This is usually unknown).
753 * @return a normal distribution containing expected received RSSI value and its variance.
754 * @throws IndoorException if something fails.
755 */
756 public static MultivariateNormalDist propagateVariancesToRssiVarianceSecondOrderNonLinear2D(
757 final double fingerprintRssi, final double pathLossExponent, final Point2D fingerprintPosition,
758 final Point2D radioSourcePosition, final Point2D estimatedPosition, final Double fingerprintRssiVariance,
759 final Double pathLossExponentVariance, final Matrix fingerprintPositionCovariance,
760 final Matrix radioSourcePositionCovariance, final Matrix estimatedPositionCovariance)
761 throws IndoorException {
762
763 if (fingerprintPosition == null || radioSourcePosition == null || estimatedPosition == null) {
764 return null;
765 }
766
767 // 2nd order Taylor expression of received power in 2D:
768 // Pr(pi) = Pr(p1)
769 // - 10*n*(x1 - xa)/(ln(10)*d1a^2)*(xi - x1)
770 // - 10*n*(y1 - ya)/(ln(10)*d1a^2)*(yi - y1)
771 // - 5*n*((y1 - ya)^2 - (x1 - xa)^2)/(ln(10)*d1a^4)*(xi - x1)^2
772 // - 5*n*((x1 - xa)^2 - (y1 - ya)^2)/(ln(10)*d1a^4)*(yi - y1)^2
773 // + 20*n*(x1 - xa)*(y1 - ya)/(ln(10)*d1a^4))*(xi - x1)*(yi - y1)
774 // where d1a^2 = (x1 - xa)^2 + (y1 - ya)^2
775
776 final var x1 = fingerprintPosition.getInhomX();
777 final var y1 = fingerprintPosition.getInhomY();
778
779 final var xa = radioSourcePosition.getInhomX();
780 final var ya = radioSourcePosition.getInhomY();
781
782 final var xi = estimatedPosition.getInhomX();
783 final var yi = estimatedPosition.getInhomY();
784
785 final var mean = new double[]{
786 fingerprintRssi, pathLossExponent, x1, y1, xa, ya, xi, yi
787 };
788 final var covariance = Matrix.diagonal(new double[]{
789 fingerprintRssiVariance != null ? fingerprintRssiVariance : 0.0,
790 pathLossExponentVariance != null ? pathLossExponentVariance : 0.0,
791 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
792 });
793
794 if (fingerprintPositionCovariance != null
795 && fingerprintPositionCovariance.getRows() == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH
796 && fingerprintPositionCovariance.getColumns() == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH) {
797
798 covariance.setSubmatrix(2, 2, 3, 3,
799 fingerprintPositionCovariance);
800 }
801
802 if (radioSourcePositionCovariance != null
803 && radioSourcePositionCovariance.getRows() == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH
804 && radioSourcePositionCovariance.getColumns() == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH) {
805 covariance.setSubmatrix(4, 4, 5, 5,
806 radioSourcePositionCovariance);
807 }
808
809 if (estimatedPositionCovariance != null
810 && estimatedPositionCovariance.getRows() == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH
811 && estimatedPositionCovariance.getColumns() == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH) {
812 covariance.setSubmatrix(6, 6, 7, 7,
813 estimatedPositionCovariance);
814 }
815
816 try {
817 return MultivariateNormalDist.propagate(new MultivariateNormalDist.JacobianEvaluator() {
818 @Override
819 public void evaluate(final double[] x, final double[] y, final Matrix jacobian) {
820
821 // Pr(pi) = Pr(p1)
822 // - 10*n*(x1 - xa)/(ln(10)*d1a^2)*(xi - x1)
823 // - 10*n*(y1 - ya)/(ln(10)*d1a^2)*(yi - y1)
824 // - 5*n*((y1 - ya)^2 - (x1 - xa)^2)/(ln(10)*d1a^4)*(xi - x1)^2
825 // - 5*n*((x1 - xa)^2 - (y1 - ya)^2)/(ln(10)*d1a^4)*(yi - y1)^2
826 // + 20*n*(x1 - xa)*(y1 - ya)/(ln(10)*d1a^4))*(xi - x1)*(yi - y1)
827 // where d1a^2 = (x1 - xa)^2 + (y1 - ya)^2
828
829 // Hence:
830 // Pr(pi) = Pr(p1)
831 // - 10*n*(x1 - xa)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2))*(xi - x1)
832 // - 10*n*(y1 - ya)/(ln(10)*(x1 - xa)^2 + (y1 - ya)^2)*(yi - y1)
833 // - 5*n*((y1 - ya)^2 - (x1 - xa)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2)^2))*(xi - x1)^2
834 // - 5*n*((x1 - xa)^2 - (y1 - ya)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2)^2))*(yi - y1)^2
835 // + 20*n*(x1 - xa)*(y1 - ya)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2)^2))*(xi - x1)*(yi - y1)
836
837 final var diffX1a = x1 - xa;
838 final var diffY1a = y1 - ya;
839
840 final var diffXi1 = xi - x1;
841 final var diffYi1 = yi - y1;
842
843 final var diffX1a2 = diffX1a * diffX1a;
844 final var diffY1a2 = diffY1a * diffY1a;
845
846 final var diffXi12 = diffXi1 * diffXi1;
847 final var diffYi12 = diffYi1 * diffYi1;
848
849 final var d1a2 = diffX1a2 + diffY1a2;
850 final var d1a4 = d1a2 * d1a2;
851 final var d1a8 = d1a4 * d1a4;
852
853 final var ln10 = Math.log(10.0);
854
855 y[0] = fingerprintRssi - 10.0 * pathLossExponent * diffX1a / (ln10 * d1a2) * diffXi1
856 - 10.0 * pathLossExponent * diffY1a / (ln10 * d1a2) * diffYi1
857 - 5.0 * pathLossExponent * (-diffX1a2 + diffY1a2) / (ln10 * d1a4) * diffXi12
858 - 5.0 * pathLossExponent * (diffX1a2 - diffY1a2) / (ln10 * d1a4) * diffYi12
859 + 20.0 * pathLossExponent * diffX1a * diffY1a / (ln10 * d1a4) * diffXi1 * diffYi1;
860
861 // compute gradient (is a jacobian having 1 row and 8 columns)
862
863
864 // derivative of rssi respect to fingerprint rssi
865 final var derivativeFingerprintRssi = 1.0;
866
867 // derivative of rssi respect to path-loss exponent
868
869 // diff(Pr(pi))/diff(n) = -10*(x1 - xa)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2))*(xi - x1)
870 // -10*(y1 - ya)/(ln(10)*(x1 - xa)^2 + (y1 - ya)^2)*(yi - y1)
871 // -5*((y1 - ya)^2 - (x1 - xa)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2)^2))*(xi - x1)^2
872 // -5*((x1 - xa)^2 - (y1 - ya)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2)^2))*(yi - y1)^2
873 // +20*(x1 - xa)*(y1 - ya)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2)^2))*(xi - x1)*(yi - y1)
874 final var derivativePathLossExponent = -10.0 * diffX1a / (ln10 * d1a2) * diffXi1
875 - 10.0 * diffY1a / (ln10 * d1a2) * diffYi1
876 - 5.0 * (-diffX1a2 + diffY1a2) / (ln10 * d1a4) * diffXi12
877 - 5.0 * (diffX1a2 - diffY1a2) / (ln10 * d1a4) * diffYi12
878 + 20.0 * diffX1a * diffY1a / (ln10 * d1a4) * diffXi1 * diffYi1;
879
880 // derivative of rssi respect to x1
881
882 // We have
883 // Pr(pi) = Pr(p1)
884 // - 10*n*(x1 - xa)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2))*(xi - x1)
885 // - 10*n*(y1 - ya)/(ln(10)*(x1 - xa)^2 + (y1 - ya)^2)*(yi - y1)
886 // - 5*n*((y1 - ya)^2 - (x1 - xa)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2)^2))*(xi - x1)^2
887 // - 5*n*((x1 - xa)^2 - (y1 - ya)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2)^2))*(yi - y1)^2
888 // + 20*n*(x1 - xa)*(y1 - ya)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2)^2))*(xi - x1)*(yi - y1)
889
890 // Pr(pi) = Pr(p1)
891 // - 10*n*((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2))
892 // + 5*n*((x1 - xa)^2 - (y1 - ya)^2)*((xi - x1)^2 - (yi - y1)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2)^2)
893 // + 20*n*(x1 - xa)*(y1 - ya)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2)^2))*(xi - x1)*(yi - y1)
894
895 // and we know that: (f(x)/g(x))' = (f'(x)*g(x) - f(x)*g'(x))/g(x)^2
896 // and also that (f(x)*g(x))' = f'(x)*g(x) + f(x)*g'(x)
897
898 // Hence
899 // diff((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))/diff(x1) =
900 // diff(x1*xi -xa*xi -x1^2 + xa*x1)/diff(x1) =
901 // diff(-x1^2 + (xi + xa)*x1 - xa*xi)/diff(x1) =
902 // -2*x1 + xi + xa
903
904 // diff(((x1 - xa)^2 - (y1 - ya)^2)*((xi - x1)^2 - (yi - y1)^2))/diff(x1) =
905 // 2*(x1 - xa)*((xi - x1)^2 - (yi - y1)^2) - 2*(xi - x1)*((x1 - xa)^2 - (y1 - ya)^2)
906
907 // diff((x1 - xa)*(y1 - ya))/diff(x1) =
908 // y1 - ya
909
910 // diff((xi - x1)*(yi - y1))/diff(x1) =
911 // -(yi - y1)
912
913 // diff((x1 - xa)*(y1 - ya)*(xi - x1)*(yi - y1))/diff(x1) =
914 // (y1 - ya)*(xi - x1)*(yi - y1) - (x1 - xa)*(y1 - ya)*(yi - y1) =
915 // ((y1 - ya)*(xi - x1) - (x1 - xa)*(y1 - ya))*(yi - y1)
916
917 // diff(Pr(pi))/diff(x1) = -10*n/ln(10)*((-2*x1 + xi + xa)*((x1 - xa)^2 + (y1 - ya)^2) - ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))*2*(x1 - xa))/((x1 - xa)^2 + (y1 - ya)^2)^2
918 // + 5*n/ln(10)*(2*((x1 - xa)*((xi - x1)^2 - (yi - y1)^2) - (xi - x1)*((x1 - xa)^2 - (y1 - ya)^2))*((x1 - xa)^2 + (y1 - ya)^2)^2 - ((x1 - xa)^2 - (y1 - ya)^2)*((xi - x1)^2 - (yi - y1)^2))*2*((x1 - xa)^2 + (y1 - ya)^2)*2*(x1 - xa))/((x1 - xa)^2 + (y1 - ya)^2)^4
919 // + 20*n/ln(10)*(((y1 - ya)*(xi - x1) - (x1 - xa)*(y1 - ya))*(yi - y1)*((x1 - xa)^2 + (y1 - ya)^2)^2 - (x1 - xa)*(y1 - ya)*(xi - x1)*(yi - y1)*2*((x1 - xa)^2 + (y1 - ya)^2)*2*(x1 - xa))/((x1 - xa)^2 + (y1 - ya)^2)^4
920
921 // diff(Pr(pi))/diff(x1) = -10*n/ln(10)*((-2*x1 + xi + xa)*d1a^2 - ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))*2*(x1 - xa))/d1a^4
922 // + 5*n/ln(10)*(2*((x1 - xa)*((xi - x1)^2 - (yi - y1)^2) - (xi - x1)*((x1 - xa)^2 - (y1 - ya)^2))*d1a^4 - ((x1 - xa)^2 - (y1 - ya)^2)*((xi - x1)^2 - (yi - y1)^2))*4*d1a^2*(x1 - xa))/d1a^8
923 // + 20*n/ln(10)*(((y1 - ya)*(xi - x1) - (x1 - xa)*(y1 - ya))*(yi - y1)*d1a^4 - (x1 - xa)*(y1 - ya)*(xi - x1)*(yi - y1)*4*d1a^2*(x1 - xa))/d1a^8
924 final var crossDiff = diffX1a * diffXi1 + diffY1a * diffYi1;
925 final var tmpX = crossDiff * 2.0 * diffX1a;
926 final var tmpX2 = (diffX1a2 - diffY1a2) * (diffXi12 - diffYi12) * 4.0 * d1a2 * diffX1a;
927 final var tmpX3 = diffX1a * diffY1a * diffXi1 * diffYi1 * 4.0 * d1a2 * diffX1a;
928 final var derivativeX1 = -10.0 * pathLossExponent / ln10 * ((-2.0 * x1 + xi + xa) * d1a2 - tmpX) / d1a4
929 + 5.0 * pathLossExponent / ln10 * (2.0 * (diffX1a * (diffXi12 - diffYi12) - diffXi1 * (diffX1a2 - diffY1a2)) * d1a4 - tmpX2) / d1a8
930 + 20.0 * pathLossExponent / ln10 * ((diffY1a * diffXi1 - diffX1a * diffY1a) * diffYi1 * d1a4 - tmpX3) / d1a8;
931
932 // derivative of rssi respect to y1
933
934 // We have
935 // Pr(pi) = Pr(p1)
936 // - 10*n*(x1 - xa)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2))*(xi - x1)
937 // - 10*n*(y1 - ya)/(ln(10)*(x1 - xa)^2 + (y1 - ya)^2)*(yi - y1)
938 // - 5*n*((y1 - ya)^2 - (x1 - xa)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2)^2))*(xi - x1)^2
939 // - 5*n*((x1 - xa)^2 - (y1 - ya)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2)^2))*(yi - y1)^2
940 // + 20*n*(x1 - xa)*(y1 - ya)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2)^2))*(xi - x1)*(yi - y1)
941
942 // Pr(pi) = Pr(p1)
943 // - 10*n*((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2))
944 // + 5*n*((x1 - xa)^2 - (y1 - ya)^2)*((xi - x1)^2 - (yi - y1)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2)^2)
945 // + 20*n*(x1 - xa)*(y1 - ya)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2)^2))*(xi - x1)*(yi - y1)
946
947 // and we know that: (f(x)/g(x))' = (f'(x)*g(x) - f(x)*g'(x))/g(x)^2
948 // and also that (f(x)*g(x))' = f'(x)*g(x) + f(x)*g'(x)
949
950 // Hence
951 // diff((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))/diff(y1) =
952 // diff(y1*yi -ya*yi -y1^2 + ya*y1)/diff(y1) =
953 // diff(-y1^2 + (yi + ya)*y1 - ya*yi)/diff(y1) =
954 // -2*y1 + yi + ya
955
956 // diff(((x1 - xa)^2 - (y1 - ya)^2)*((xi - x1)^2 - (yi - y1)^2))/diff(y1) =
957 // -2*(y1 - ya)*((xi - x1)^2 - (yi - y1)^2) + 2*(yi - y1)*((x1 - xa)^2 - (y1 - ya)^2)
958
959 // diff((x1 - xa)*(y1 - ya))/diff(y1) =
960 // x1 - xa
961
962 // diff((xi - x1)*(yi - y1))/diff(y1) =
963 // -(xi - x1)
964
965 // diff((x1 - xa)*(y1 - ya)*(xi - x1)*(yi - y1))/diff(y1) =
966 // (x1 - xa)*(xi - x1)*(yi - y1) - (x1 - xa)*(y1 - ya)*(xi - x1) =
967 // ((x1 - xa)*(yi - y1) - (x1 - xa)*(y1 - ya))*(xi - x1)
968
969 // diff(Pr(pi))/diff(y1) = -10*n/ln(10)*((-2*y1 + yi + ya)*((x1 - xa)^2 + (y1 - ya)^2) - ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))*2*(y1 - ya))/((x1 - xa)^2 + (y1 - ya)^2)^2
970 // + 5*n/ln(10)*(2*(-(y1 - ya)*((xi - x1)^2 - (yi - y1)^2) + (yi - y1)*((x1 - xa)^2 - (y1 - ya)^2))*((x1 - xa)^2 + (y1 - ya)^2)^2 - ((x1 - xa)^2 - (y1 - ya)^2)*((xi - x1)^2 - (yi - y1)^2)*2*((x1 - xa)^2 + (y1 - ya)^2)*2*(y1 - ya))/((x1 - xa)^2 + (y1 - ya)^2)^4
971 // + 20*n/ln(10)*(((x1 - xa)*(yi - y1) - (x1 - xa)*(y1 - ya))*(xi - x1)*((x1 - xa)^2 + (y1 - ya)^2)^2 - (x1 - xa)*(y1 - ya)*(xi - x1)*(yi - y1)*2*((x1 - xa)^2 + (y1 - ya)^2)*2*(y1 - ya))/((x1 - xa)^2 + (y1 - ya)^2)^4
972
973 // diff(Pr(pi))/diff(y1) = -10*n/ln(10)*((-2*y1 + yi + ya)*d1a^2 - ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))*2*(y1 - ya))/d1a^4
974 // + 5*n/ln(10)*(2*(-(y1 - ya)*((xi - x1)^2 - (yi - y1)^2) + (yi - y1)*((x1 - xa)^2 - (y1 - ya)^2))*d1a^4 - ((x1 - xa)^2 - (y1 - ya)^2)*((xi - x1)^2 - (yi - y1)^2)*4*d1a^2*(y1 - ya))/d1a^8
975 // + 20*n/ln(10)*(((x1 - xa)*(yi - y1) - (x1 - xa)*(y1 - ya))*(xi - x1)*d1a^4 - (x1 - xa)*(y1 - ya)*(xi - x1)*(yi - y1)*4*d1a^2*(y1 - ya))/d1a^8
976 final var tmpY = crossDiff * 2.0 * diffY1a;
977 final var tmpY2 = (diffX1a2 - diffY1a2) * (diffXi12 - diffYi12) * 4.0 * d1a2 * diffY1a;
978 final var tmpY3 = diffX1a * diffY1a * diffXi1 * diffYi1 * 4.0 * d1a2 * diffY1a;
979 final var derivativeY1 = -10.0 * pathLossExponent / ln10 * ((-2.0 * y1 + yi + ya) * d1a2 - tmpY) / d1a4
980 + 5.0 * pathLossExponent / ln10 * (2.0 * (-diffY1a * (diffXi12 - diffYi12) + diffYi1 * (diffX1a2 - diffY1a2)) * d1a4 - tmpY2) / d1a8
981 + 20.0 * pathLossExponent / ln10 * ((diffX1a * diffYi1 - diffX1a * diffY1a) * diffXi1 * d1a4 - tmpY3) / d1a8;
982
983 // derivative of rssi respect to xa
984
985 // Pr(pi) = Pr(p1)
986 // - 10*n*((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2))
987 // + 5*n*((x1 - xa)^2 - (y1 - ya)^2)*((xi - x1)^2 - (yi - y1)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2)^2)
988 // + 20*n*(x1 - xa)*(y1 - ya)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2)^2))*(xi - x1)*(yi - y1)
989
990 // diff((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))/diff(xa) =
991 // -(xi - x1)
992
993 // diff(((x1 - xa)^2 - (y1 - ya)^2)*((xi - x1)^2 - (yi - y1)^2))/diff(xa) =
994 // -2*(x1 - xa)*((xi - x1)^2 - (yi - y1)^2)
995
996 // diff((x1 - xa)*(y1 - ya)*(xi - x1)*(yi - y1))/diff(xa) =
997 // -(y1 - ya)*(xi - x1)*(yi - y1)
998
999 // diff(Pr(pi))/diff(xa) = -10*n/ln(10)*(-(xi - x1)*((x1 - xa)^2 + (y1 - ya)^2) - ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))*-2*(x1 - xa))/((x1 - xa)^2 + (y1 - ya)^2)^2
1000 // + 5*n/ln(10)*(-2*(x1 - xa)*((xi - x1)^2 - (yi - y1)^2)*((x1 - xa)^2 + (y1 - ya)^2)^2 - ((x1 - xa)^2 - (y1 - ya)^2)*((xi - x1)^2 - (yi - y1)^2)*2*((x1 - xa)^2 + (y1 - ya)^2)*-2*(x1 - xa))/((x1 - xa)^2 + (y1 - ya)^2)^4
1001 // + 20*n*(-(y1 - ya)*(xi - x1)*(yi - y1)*((x1 - xa)^2 + (y1 - ya)^2)^2 - (x1 - xa)*(y1 - ya)*(xi - x1)*(yi - y1)*2*((x1 - xa)^2 + (y1 - ya)^2)*-2*(x1 - xa))/((x1 - xa)^2 + (y1 - ya)^2)^4
1002
1003 // diff(Pr(pi))/diff(xa) = -10*n/ln(10)*(-(xi - x1)*d1a^2 + ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))*2*(x1 - xa))/d1a^4
1004 // + 5*n/ln(10)*(-2*(x1 - xa)*((xi - x1)^2 - (yi - y1)^2)*d1a^4 + ((x1 - xa)^2 - (y1 - ya)^2)*((xi - x1)^2 - (yi - y1)^2)*4*d1a^2*(x1 - xa))/d1a^8
1005 // + 20*n/ln(10)*(-(y1 - ya)*(xi - x1)*(yi - y1)*d1a^4 + (x1 - xa)*(y1 - ya)*(xi - x1)*(yi - y1)*4*d1a^2*(x1 - xa))/d1a^8
1006 final var derivativeXa = -10.0 * pathLossExponent / ln10 * (-diffXi1 * d1a2 + tmpX) / d1a4
1007 + 5.0 * pathLossExponent / ln10 * (-2.0 * diffX1a * (diffXi12 - diffYi12) * d1a4 + tmpX2) / d1a8
1008 + 20.0 * pathLossExponent / ln10 * (-diffY1a * diffXi1 * diffYi1 * d1a4 + tmpX3) / d1a8;
1009
1010 // derivative of rssi respect to ya
1011
1012 // Pr(pi) = Pr(p1)
1013 // - 10*n*((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2))
1014 // + 5*n*((x1 - xa)^2 - (y1 - ya)^2)*((xi - x1)^2 - (yi - y1)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2)^2)
1015 // + 20*n*(x1 - xa)*(y1 - ya)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2)^2))*(xi - x1)*(yi - y1)
1016
1017 // diff((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))/diff(ya) =
1018 // -(yi - y1)
1019
1020 // diff(((x1 - xa)^2 - (y1 - ya)^2)*((xi - x1)^2 - (yi - y1)^2))/diff(ya) =
1021 // 2*(y1 - ya)*((xi - x1)^2 - (yi - y1)^2)
1022
1023 // diff((x1 - xa)*(y1 - ya)*(xi - x1)*(yi - y1))/diff(ya) =
1024 // -(x1 - xa)*(xi - x1)*(yi - y1)
1025
1026 // diff(Pr(pi))/diff(ya) = -10*n/ln(10)*(-(yi - y1)*((x1 - xa)^2 + (y1 - ya)^2) - ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))*-2*(y1 - ya))/((x1 - xa)^2 + (y1 - ya)^2)^2
1027 // + 5*n/ln(10)*(2*(y1 - ya)*((xi - x1)^2 - (yi - y1)^2)*((x1 - xa)^2 + (y1 - ya)^2)^2 - ((x1 - xa)^2 - (y1 - ya)^2)*((xi - x1)^2 - (yi - y1)^2)*2*((x1 - xa)^2 + (y1 - ya)^2)*-2*(y1 - ya))/((x1 - xa)^2 + (y1 - ya)^2)^4
1028 // + 20*n/ln(10)*(-(x1 - xa)*(xi - x1)*(yi - y1)*((x1 - xa)^2 + (y1 - ya)^2)^2 - (x1 - xa)*(y1 - ya)*(xi - x1)*(yi - y1)*2*((x1 - xa)^2 + (y1 - ya)^2)*-2(y1 - ya))/((x1 - xa)^2 + (y1 - ya)^2)^4
1029
1030 // diff(Pr(pi))/diff(ya) = -10*n/ln(10)*(-(yi - y1)*d1a^2 + ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))*2*(y1 - ya))/d1a^4
1031 // + 5*n/ln(10)*(2*(y1 - ya)*((xi - x1)^2 - (yi - y1)^2)*d1a^4 + ((x1 - xa)^2 - (y1 - ya)^2)*((xi - x1)^2 - (yi - y1)^2)*4*d1a^2*(y1 - ya))/d1a^8
1032 // + 20*n/ln(10)*(-(x1 - xa)*(xi - x1)*(yi - y1)*d1a^4 + (x1 - xa)*(y1 - ya)*(xi - x1)*(yi - y1)*4*d1a^2*(y1 - ya))/d1a^8
1033 final var derivativeYa = -10.0 * pathLossExponent / ln10 * (-diffYi1 * d1a2 + tmpY) / d1a4
1034 + 5.0 * pathLossExponent / ln10 * (2.0 * diffY1a * (diffXi12 - diffYi12) * d1a4 + tmpY2) / d1a8
1035 + 20.0 * pathLossExponent / ln10 * (-diffX1a * diffXi1 * diffYi1 * d1a4 + tmpY3) / d1a8;
1036
1037 // derivative of rssi respect to xi
1038
1039 // Pr(pi) = Pr(p1)
1040 // - 10*n*((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2))
1041 // + 5*n*((x1 - xa)^2 - (y1 - ya)^2)*((xi - x1)^2 - (yi - y1)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2)^2)
1042 // + 20*n*(x1 - xa)*(y1 - ya)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2)^2))*(xi - x1)*(yi - y1)
1043
1044 // diff((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))/diff(xi) =
1045 // x1 - xa
1046
1047 // diff(((x1 - xa)^2 - (y1 - ya)^2)*((xi - x1)^2 - (yi - y1)^2))/diff(xi) =
1048 // 2*((x1 - xa)^2 - (y1 - ya)^2)*(xi - x1)
1049
1050 // diff((x1 - xa)*(y1 - ya)*(xi - x1)*(yi - y1))/diff(xi) =
1051 // (x1 - xa)*(y1 - ya)*(yi - y1)
1052
1053 // diff(Pr(pi))/diff(xi) = -10*n/ln(10)*((x1 - xa)*((x1 - xa)^2 + (y1 - ya)^2) - ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))*0)/((x1 - xa)^2 + (y1 - ya)^2)^2
1054 // + 5*n/ln(10)*(2*((x1 - xa)^2 - (y1 - ya)^2)*(xi - x1)*((x1 - xa)^2 + (y1 - ya)^2)^2 - ((x1 - xa)^2 - (y1 - ya)^2)*((xi - x1)^2 - (yi - y1)^2)*0)/((x1 - xa)^2 + (y1 - ya)^2)^4
1055 // + 20*n/ln(10)*((x1 - xa)*(y1 - ya)*(yi - y1)*((x1 - xa)^2 + (y1 - ya)^2)^2 - (x1 - xa)*(y1 - ya)*(xi - x1)*(yi - y1)*0)/((x1 - xa)^2 + (y1 - ya)^2)^4
1056
1057 // diff(Pr(pi))/diff(xi) = -10*n/ln(10)*((x1 - xa)*d1a^2)/d1a^4
1058 // + 5*n/ln(10)*(2*((x1 - xa)^2 - (y1 - ya)^2)*(xi - x1)*d1a^4)/d1a^8
1059 // + 20*n/ln(10)*((x1 - xa)*(y1 - ya)*(yi - y1)*d1a^4)/d1a^8
1060
1061 // diff(Pr(pi))/diff(xi) = -10*n/ln(10)*(x1 - xa)/d1a^2
1062 // + 10*n/ln(10)*(((x1 - xa)^2 - (y1 - ya)^2)*(xi - x1))/d1a^4
1063 // + 20*n/ln(10)*(x1 - xa)*(y1 - ya)*(yi - y1)/d1a^4
1064 final var derivativeXi = -10.0 * pathLossExponent / ln10 * diffX1a / d1a2
1065 + 10.0 * pathLossExponent / ln10 * ((diffX1a2 - diffY1a2) * diffXi1) / d1a4
1066 + 20.0 * pathLossExponent / ln10 * diffX1a * diffY1a * diffYi1 / d1a4;
1067
1068 // derivative of rssi respect to yi
1069
1070 // Pr(pi) = Pr(p1)
1071 // - 10*n*((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2))
1072 // + 5*n*((x1 - xa)^2 - (y1 - ya)^2)*((xi - x1)^2 - (yi - y1)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2)^2)
1073 // + 20*n*(x1 - xa)*(y1 - ya)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2)^2))*(xi - x1)*(yi - y1)
1074
1075 // diff((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))/diff(yi) =
1076 // y1 - ya
1077
1078 // diff(((x1 - xa)^2 - (y1 - ya)^2)*((xi - x1)^2 - (yi - y1)^2))/diff(yi) =
1079 // 2*((x1 - xa)^2 - (y1 - ya)^2)*(yi - y1)
1080
1081 // diff((x1 - xa)*(y1 - ya)*(xi - x1)*(yi - y1))/diff(yi) =
1082 // (x1 - xa)*(y1 - ya)*(xi - x1)
1083
1084 // diff(Pr(pi))/diff(yi) = -10*n/ln(10)*((y1 - ya)*((x1 - xa)^2 + (y1 - ya)^2) - ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1))*0)/((x1 - xa)^2 + (y1 - ya)^2)^2
1085 // + 5*n/ln(10)*(2*((x1 - xa)^2 - (y1 - ya)^2)*(yi - y1)*((x1 - xa)^2 + (y1 - ya)^2)^2 - ((x1 - xa)^2 - (y1 - ya)^2)*((xi - x1)^2 - (yi - y1)^2)*0)/((x1 - xa)^2 + (y1 - ya)^2)^4
1086 // + 20*n/ln(10)*((x1 - xa)*(y1 - ya)*(xi - x1)*((x1 - xa)^2 + (y1 - ya)^2)^2 - (x1 - xa)*(y1 - ya)*(xi - x1)*(yi - y1)*0)/((x1 - xa)^2 + (y1 - ya)^2)^4
1087
1088 // diff(Pr(pi))/diff(yi) = -10*n/ln(10)*((y1 - ya)*d1a^2)/d1a^4
1089 // + 5*n/ln(10)*(2*((x1 - xa)^2 - (y1 - ya)^2)*(yi - y1)*d1a^4)/d1a^8
1090 // + 20*n/ln(10)*((x1 - xa)*(y1 - ya)*(xi - x1)*d1a^4)/d1a^8
1091
1092 // diff(Pr(pi))/diff(yi) = -10*n/ln(10)*(y1 - ya)/d1a^2
1093 // + 10*n/ln(10)*(((x1 - xa)^2 - (y1 - ya)^2)*(yi - y1))/d1a^4
1094 // + 20*n/ln(10)*(x1 - xa)*(y1 - ya)*(xi - x1)/d1a^4
1095 final var derivativeYi = -10.0 * pathLossExponent / ln10 * diffY1a / d1a2
1096 + 10.0 * pathLossExponent / ln10 * ((diffX1a2 - diffY1a2) * diffYi1) / d1a4
1097 + 20.0 * pathLossExponent / ln10 * diffX1a * diffY1a * diffXi1 / d1a4;
1098
1099 // set derivatives fingerprintRssi, pathLossExponent, x1, y1, xa, ya, xi, yi
1100 jacobian.setElementAtIndex(0, derivativeFingerprintRssi);
1101 jacobian.setElementAtIndex(1, derivativePathLossExponent);
1102 jacobian.setElementAtIndex(2, derivativeX1);
1103 jacobian.setElementAtIndex(3, derivativeY1);
1104 jacobian.setElementAtIndex(4, derivativeXa);
1105 jacobian.setElementAtIndex(5, derivativeYa);
1106 jacobian.setElementAtIndex(6, derivativeXi);
1107 jacobian.setElementAtIndex(7, derivativeYi);
1108 }
1109
1110 @Override
1111 public int getNumberOfVariables() {
1112 return 1;
1113 }
1114 }, mean, covariance);
1115 } catch (final AlgebraException | StatisticsException e) {
1116 throw new IndoorException(e);
1117 }
1118 }
1119
1120 /**
1121 * Propagates provided variances (fingerprint rssi variance, path-loss exponent variance,
1122 * fingerprint position covariance and radio source position covariance) into
1123 * rssi variance by considering the 3D 1st order Taylor expression of received power.
1124 * Notice that any unknown variance is assumed to be zero.
1125 *
1126 * @param fingerprintRssi closest located fingerprint reading RSSI expressed in dBm's.
1127 * @param pathLossExponent path-loss exponent.
1128 * @param fingerprintPosition position of closest fingerprint.
1129 * @param radioSourcePosition radio source position associated to fingerprint reading.
1130 * @param estimatedPosition position to be estimated. Usually this is equal to the
1131 * initial position used by a non-linear algorithm.
1132 * @param fingerprintRssiVariance variance of fingerprint RSSI or null if unknown.
1133 * @param pathLossExponentVariance variance of path-loss exponent or null if unknown.
1134 * @param fingerprintPositionCovariance covariance of fingerprint position or null if
1135 * unknown.
1136 * @param radioSourcePositionCovariance covariance of radio source position or null
1137 * if unknown.
1138 * @param estimatedPositionCovariance covariance of position to be estimated or null
1139 * if unknown. (This is usually unknown).
1140 * @return a normal distribution containing expected received RSSI value and its variance.
1141 * @throws IndoorException if something fails.
1142 */
1143 public static MultivariateNormalDist propagateVariancesToRssiVarianceSecondOrderNonLinear3D(
1144 final double fingerprintRssi, final double pathLossExponent, final Point3D fingerprintPosition,
1145 final Point3D radioSourcePosition, final Point3D estimatedPosition, final Double fingerprintRssiVariance,
1146 final Double pathLossExponentVariance, final Matrix fingerprintPositionCovariance,
1147 final Matrix radioSourcePositionCovariance, final Matrix estimatedPositionCovariance)
1148 throws IndoorException {
1149
1150 if (fingerprintPosition == null || radioSourcePosition == null || estimatedPosition == null) {
1151 return null;
1152 }
1153
1154 // 2nd order Taylor expression of received power in 3D:
1155 // Pr(pi) = Pr(p1)
1156 // - 10*n*(x1 - xa)/(ln(10)*d1a^2)*(xi - x1)
1157 // - 10*n*(y1 - ya)/(ln(10)*d1a^2)*(yi - y1)
1158 // - 10*n*(z1 - za)/(ln(10)*d1a^2)*(zi - z1)
1159 // - 5*n*((y1 - ya)^2 + (z1 - za)^2) - (x1 - xa)^2)/(ln(10)*d1a^4)*(xi - x1)^2
1160 // - 5*n*((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)/(ln(10)*d1a^4)*(yi - y1)^2
1161 // - 5*n*((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)/(ln(10)*d1a^4)*(zi - z1)^2
1162 // + 20*n*(x1 - xa)*(y1 - ya)/(ln(10)*d1a^4)*(xi - x1)*(yi - y1)
1163 // + 20*n*(y1 - ya)*(z1 - za)/(ln(10)*d1a^4)*(yi - y1)*(zi - z1)
1164 // + 20*n*(x1 - xa)*(z1 - za)/(ln(10)*d1a^4)*(xi - x1)*(zi - z1)
1165 // where d1a^2 = (x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2
1166
1167 final var x1 = fingerprintPosition.getInhomX();
1168 final var y1 = fingerprintPosition.getInhomY();
1169 final var z1 = fingerprintPosition.getInhomZ();
1170
1171 final var xa = radioSourcePosition.getInhomX();
1172 final var ya = radioSourcePosition.getInhomY();
1173 final var za = radioSourcePosition.getInhomZ();
1174
1175 final var xi = estimatedPosition.getInhomX();
1176 final var yi = estimatedPosition.getInhomY();
1177 final var zi = estimatedPosition.getInhomZ();
1178
1179 final var mean = new double[]{
1180 fingerprintRssi, pathLossExponent, x1, y1, z1, xa, ya, za, xi, yi, zi
1181 };
1182 final var covariance = Matrix.diagonal(new double[]{
1183 fingerprintRssiVariance != null ? fingerprintRssiVariance : 0.0,
1184 pathLossExponentVariance != null ? pathLossExponentVariance : 0.0,
1185 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
1186 });
1187
1188 if (fingerprintPositionCovariance != null
1189 && fingerprintPositionCovariance.getRows() == Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH
1190 && fingerprintPositionCovariance.getColumns() == Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH) {
1191
1192 covariance.setSubmatrix(2, 2, 4, 4,
1193 fingerprintPositionCovariance);
1194 }
1195
1196 if (radioSourcePositionCovariance != null
1197 && radioSourcePositionCovariance.getRows() == Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH
1198 && radioSourcePositionCovariance.getColumns() == Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH) {
1199 covariance.setSubmatrix(5, 5, 7, 7,
1200 radioSourcePositionCovariance);
1201 }
1202
1203 if (estimatedPositionCovariance != null
1204 && estimatedPositionCovariance.getRows() == Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH
1205 && estimatedPositionCovariance.getColumns() == Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH) {
1206 covariance.setSubmatrix(8, 8, 10, 10,
1207 estimatedPositionCovariance);
1208 }
1209
1210 try {
1211 return MultivariateNormalDist.propagate(new MultivariateNormalDist.JacobianEvaluator() {
1212 @Override
1213 public void evaluate(final double[] x, final double[] y, final Matrix jacobian) {
1214
1215 // Pr(pi) = Pr(p1)
1216 // - 10*n*(x1 - xa)/(ln(10)*d1a^2)*(xi - x1)
1217 // - 10*n*(y1 - ya)/(ln(10)*d1a^2)*(yi - y1)
1218 // - 10*n*(z1 - za)/(ln(10)*d1a^2)*(zi - z1)
1219 // - 5*n*((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)/(ln(10)*d1a^4)*(xi - x1)^2
1220 // - 5*n*((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)/(ln(10)*d1a^4)*(yi - y1)^2
1221 // - 5*n*((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)/(ln(10)*d1a^4)*(zi - z1)^2
1222 // + 20*n*(x1 - xa)*(y1 - ya)/(ln(10)*d1a^4)*(xi - x1)*(yi - y1)
1223 // + 20*n*(y1 - ya)*(z1 - za)/(ln(10)*d1a^4)*(yi - y1)*(zi - z1)
1224 // + 20*n*(x1 - xa)*(z1 - za)/(ln(10)*d1a^4)*(xi - x1)*(zi - z1)
1225 // where d1a^2 = (x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2
1226
1227 // Hence:
1228 // Pr(pi) = Pr(p1)
1229 // -10*n*(x1 - xa)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2))*(xi - x1)
1230 // -10*n*(y1 - ya)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2))*(yi - y1)
1231 // -10*n*(z1 - za)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2))*(zi - z1)
1232 // -5*n*((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(xi - x1)^2
1233 // -5*n*((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(yi - y1)^2
1234 // -5*n*((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(zi - z1)^2
1235 // +20*n*(x1 - xa)*(y1 - ya)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(xi - x1)*(yi - y1)
1236 // +20*n*(y1 - ya)*(z1 - za)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(yi - y1)*(zi - z1)
1237 // +20*n*(x1 - xa)*(z1 - za)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(xi - x1)*(zi - z1)
1238
1239 final var diffX1a = x1 - xa;
1240 final var diffY1a = y1 - ya;
1241 final var diffZ1a = z1 - za;
1242
1243 final var diffXi1 = xi - x1;
1244 final var diffYi1 = yi - y1;
1245 final var diffZi1 = zi - z1;
1246
1247 final var diffX1a2 = diffX1a * diffX1a;
1248 final var diffY1a2 = diffY1a * diffY1a;
1249 final var diffZ1a2 = diffZ1a * diffZ1a;
1250
1251 final var diffXi12 = diffXi1 * diffXi1;
1252 final var diffYi12 = diffYi1 * diffYi1;
1253 final var diffZi12 = diffZi1 * diffZi1;
1254
1255 final var d1a2 = diffX1a2 + diffY1a2 + diffZ1a2;
1256 final var d1a4 = d1a2 * d1a2;
1257 final var d1a8 = d1a4 * d1a4;
1258
1259 final var ln10 = Math.log(10.0);
1260 final var crossDiff = diffX1a * diffXi1 + diffY1a * diffYi1 + diffZ1a * diffZi1;
1261
1262 // Pr(pi) = Pr(p1)
1263 // -10*n*(x1 - xa)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2))*(xi - x1)
1264 // -10*n*(y1 - ya)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2))*(yi - y1)
1265 // -10*n*(z1 - za)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2))*(zi - z1)
1266 // -5*n*((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(xi - x1)^2
1267 // -5*n*((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(yi - y1)^2
1268 // -5*n*((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(zi - z1)^2
1269 // +20*n*(x1 - xa)*(y1 - ya)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(xi - x1)*(yi - y1)
1270 // +20*n*(y1 - ya)*(z1 - za)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(yi - y1)*(zi - z1)
1271 // +20*n*(x1 - xa)*(z1 - za)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(xi - x1)*(zi - z1)
1272
1273 y[0] = fingerprintRssi - 10.0 * pathLossExponent * diffX1a / (ln10 * d1a2) * diffXi1
1274 - 10.0 * pathLossExponent * diffY1a / (ln10 * d1a2) * diffYi1
1275 - 10.0 * pathLossExponent * diffZ1a / (ln10 * d1a2) * diffZi1
1276 - 5.0 * pathLossExponent * (-diffX1a2 + diffY1a2 + diffZ1a2) / (ln10 * d1a4) * diffXi12
1277 - 5.0 * pathLossExponent * (diffX1a2 - diffY1a2 + diffZ1a2) / (ln10 * d1a4) * diffYi12
1278 - 5.0 * pathLossExponent * (diffX1a2 + diffY1a2 - diffZ1a2) / (ln10 * d1a4) * diffZi12
1279 + 20.0 * pathLossExponent * diffX1a * diffY1a / (ln10 * d1a4) * diffXi1 * diffYi1
1280 + 20.0 * pathLossExponent * diffY1a * diffZ1a / (ln10 * d1a4) * diffYi1 * diffZi1
1281 + 20.0 * pathLossExponent * diffX1a * diffZ1a / (ln10 * d1a4) * diffXi1 * diffZi1;
1282
1283 // compute gradient (is a jacobian having 1 row and 11 columns)
1284
1285 // derivative of rssi respect to fingerprint rssi
1286 final var derivativeFingerprintRssi = 1.0;
1287
1288 // derivative of rssi respect to path-loss exponent
1289
1290 // diff(Pr(pi))/diff(n) = -10*(x1 - xa)/(ln(10)*d1a^2)*(xi - x1)
1291 // -10*(y1 - ya)/(ln(10)*d1a^2)*(yi - y1)
1292 final var derivativePathLossExponent = -10.0 * diffX1a / (ln10 * d1a2) * diffXi1
1293 - 10.0 * diffY1a / (ln10 * d1a2) * diffYi1
1294 - 10.0 * diffZ1a / (ln10 * d1a2) * diffZi1
1295 - 5.0 * (-diffX1a2 + diffY1a2 + diffZ1a2) / (ln10 * d1a4) * diffXi12
1296 - 5.0 * (diffX1a2 - diffY1a2 + diffZ1a2) / (ln10 * d1a4) * diffYi12
1297 - 5.0 * (diffX1a2 + diffY1a2 - diffZ1a2) / (ln10 * d1a4) * diffZi12
1298 + 20.0 * diffX1a * diffY1a / (ln10 * d1a4) * diffXi1 * diffYi1
1299 + 20.0 * diffY1a * diffZ1a / (ln10 * d1a4) * diffYi1 * diffZi1
1300 + 20.0 * diffX1a * diffZ1a / (ln10 * d1a4) * diffXi1 * diffZi1;
1301
1302 // derivative of rssi respect to x1
1303
1304 // We have
1305 // Pr(pi) = Pr(p1)
1306 // -10*n*(x1 - xa)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2))*(xi - x1)
1307 // -10*n*(y1 - ya)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2))*(yi - y1)
1308 // -10*n*(z1 - za)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2))*(zi - z1)
1309 // -5*n*((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(xi - x1)^2
1310 // -5*n*((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(yi - y1)^2
1311 // -5*n*((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(zi - z1)^2
1312 // +20*n*(x1 - xa)*(y1 - ya)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(xi - x1)*(yi - y1)
1313 // +20*n*(y1 - ya)*(z1 - za)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(yi - y1)*(zi - z1)
1314 // +20*n*(x1 - xa)*(z1 - za)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(xi - x1)*(zi - z1)
1315
1316 // Pr(pi) = Pr(p1)
1317 // -10*n*((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2))
1318 // -5*n*((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(xi - x1)^2
1319 // -5*n*((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(yi - y1)^2
1320 // -5*n*((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(zi - z1)^2
1321 // +20*n*(x1 - xa)*(y1 - ya)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(xi - x1)*(yi - y1)
1322 // +20*n*(y1 - ya)*(z1 - za)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(yi - y1)*(zi - z1)
1323 // +20*n*(x1 - xa)*(z1 - za)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(xi - x1)*(zi - z1)
1324
1325 // and we know that: (f(x)/g(x))' = (f'(x)*g(x) - f(x)*g'(x))/g(x)^2
1326 // and also that (f(x)*g(x))' = f'(x)*g(x) + f(x)*g'(x)
1327
1328 // Hence
1329 // diff((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))/diff(x1) =
1330 // diff(x1*xi -xa*xi -x1^2 + xa*x1)/diff(x1) =
1331 // diff(-x1^2 + (xi + xa)*x1 - xa*xi)/diff(x1) =
1332 // -2*x1 + xi + xa
1333
1334 // diff(((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)*(xi - x1)^2)/diff(x1) =
1335 // diff(-(x1 - xa)^2*(xi - x1)^2)/diff(x1) =
1336 // -2*(x1 - xa)*(xi - x1)^2 - 2*(xi - x1)*(-(x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)
1337 // -2*((x1 - xa)*(xi - x1)^2 + (xi - x1)*(-(x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2))
1338
1339 // diff(((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)*(yi - y1)^2)/diff(x1) =
1340 // diff((x1 - xa)^2*(yi - y1)^2)/diff(x1) =
1341 // 2*(x1 - xa)*(yi - y1)^2
1342
1343 // diff(((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)*(z1 - za)^2)/diff(x1) =
1344 // diff((x1 - xa)^2*(z1 - za)^2)/diff(x1) =
1345 // 2*(x1 - xa)*(z1 - za)^2
1346
1347 // diff((x1 - xa)*(y1 - ya)*(xi - x1)*(yi - y1))/diff(x1) =
1348 // (y1 - ya)*(yi - y1)*((xi - x1) - (x1 - xa)) =
1349 // (y1 - ya)*(yi - y1)*(-2*x1 + xi + xa)
1350
1351 // diff((y1 - ya)*(z1 - za)*(yi - y1)*(zi - z1))/diff(x1) = 0
1352
1353 // diff((x1 - xa)*(z1 - za)*(xi - x1)*(zi - z1))/diff(x1) =
1354 // (z1 - za)*(zi - z1)*((xi - x1) - (x1 - xa)) =
1355 // (z1 - za)*(zi - z1)*(-2*x1 + xi + xa)
1356
1357 // diff(Pr(pi))/diff(x1) = -10*n/ln(10)*((-2*x1 + xi + xa)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2) - ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))*2*(x1 - xa))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2
1358 // -5*n/ln(10)*(-2*((x1 - xa)*(xi - x1)^2 + (xi - x1)*(-(x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2))*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - (((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)*(xi - x1)^2)*2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*2*(x1 - xa))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1359 // -5*n/ln(10)*(2*(x1 - xa)*(yi - y1)^2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - (((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)*(xi - x1)^2)*2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*2*(x1 - xa))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1360 // -5*n/ln(10)*(2*(x1 - xa)*(z1 - za)^2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - (((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)*(z1 - za)^2)*2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*2*(x1 - xa))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1361 // +20*n/ln(10)*((y1 - ya)*(yi - y1)*(-2*x1 + xi + xa)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - (x1 - xa)*(y1 - ya)*(xi - x1)*(yi - y1)*2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*2*(x1 - xa))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1362 // +20*n/ln(10)*(0*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - (y1 - ya)*(z1 - za)*(yi - y1)*(zi - z1)*2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*2*(x1 - xa))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1363 // +20*n/ln(10)*((z1 - za)*(zi - z1)*(-2*x1 + xi + xa)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - (x1 - xa)*(z1 - za)*(xi - x1)*(zi - z1)*2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*2*(x1 - xa))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1364
1365 // diff(Pr(pi))/diff(x1) = -10*n/ln(10)*((-2*x1 + xi + xa)*d1a^2 - ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))*2*(x1 - xa))/d1a^4
1366 // -5*n/ln(10)*(-2*((x1 - xa)*(xi - x1)^2 + (xi - x1)*(-(x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2))*d1a^4 - (((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)*(xi - x1)^2)*4*d1a^2*(x1 - xa))/d1a^8
1367 // -5*n/ln(10)*(2*(x1 - xa)*(yi - y1)^2*d1a^4 - (((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)*(xi - x1)^2)*4*d1a^2*(x1 - xa))/d1a^8
1368 // -5*n/ln(10)*(2*(x1 - xa)*(z1 - za)^2*d1a^4 - (((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)*(z1 - za)^2)*4*d1a^2*(x1 - xa))/d1a^8
1369 // +20*n/ln(10)*((y1 - ya)*(yi - y1)*(-2*x1 + xi + xa)*d1a^4 - (x1 - xa)*(y1 - ya)*(xi - x1)*(yi - y1)*4*d1a^2*(x1 - xa))/d1a^8
1370 // +20*n/ln(10)*(-(y1 - ya)*(z1 - za)*(yi - y1)*(zi - z1)*4*d1a^2*(x1 - xa))/d1a^8
1371 // +20*n/ln(10)*((z1 - za)*(zi - z1)*(-2*x1 + xi + xa)*d1a^4 - (x1 - xa)*(z1 - za)*(xi - x1)*(zi - z1)*4*d1a^2*(x1 - xa))/d1a^8
1372 final var tmp1 = (diffY1a2 + diffZ1a2 - diffX1a2) * diffXi12 * 4.0 * d1a2 * diffX1a;
1373 final var tmp2 = diffX1a * diffY1a * diffXi1 * diffYi1 * 4.0 * d1a2 * diffX1a;
1374 final var tmp3 = diffX1a * diffZ1a * diffXi1 * diffZi1 * 4.0 * d1a2 * diffX1a;
1375 final var derivativeX1 = -10.0 * pathLossExponent / ln10 * ((-2 * x1 + xi + xa) * d1a2 - crossDiff * 2.0 * diffX1a) / d1a4
1376 - 5.0 * pathLossExponent / ln10 * (-2.0 * (diffX1a * diffXi12 + diffXi1 * (-diffX1a2 + diffY1a2 + diffZ1a2)) * d1a4 - tmp1) / d1a8
1377 - 5.0 * pathLossExponent / ln10 * (2.0 * diffX1a * diffYi12 * d1a4 - tmp1) / d1a8
1378 - 5.0 * pathLossExponent / ln10 * (2.0 * diffX1a * diffZ1a2 * d1a4 - (diffX1a2 + diffY1a2 - diffZ1a2) * diffZ1a2 * 4.0 * d1a2 * diffX1a) / d1a8
1379 + 20.0 * pathLossExponent / ln10 * (diffY1a * diffYi1 * (-2.0 * x1 + xi + xa) * d1a4 - tmp2) / d1a8
1380 + 20.0 * pathLossExponent / ln10 * (-diffY1a * diffZ1a * diffYi1 * diffZi1 * 4.0 * d1a2 * diffX1a) / d1a8
1381 + 20.0 * pathLossExponent / ln10 * (diffZ1a * diffZi1 * (-2.0 * x1 + xi + xa) * d1a4 - tmp3) / d1a8;
1382
1383 // derivative of rssi respect to y1
1384
1385 // We have
1386 // Pr(pi) = Pr(p1)
1387 // -10*n*((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2))
1388 // -5*n*((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(xi - x1)^2
1389 // -5*n*((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(yi - y1)^2
1390 // -5*n*((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(zi - z1)^2
1391 // +20*n*(x1 - xa)*(y1 - ya)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(xi - x1)*(yi - y1)
1392 // +20*n*(y1 - ya)*(z1 - za)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(yi - y1)*(zi - z1)
1393 // +20*n*(x1 - xa)*(z1 - za)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(xi - x1)*(zi - z1)
1394
1395 // Hence
1396 // diff((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))/diff(y1) =
1397 // diff(y1*yi - ya*yi -y1^2 + ya*y1)/diff(y1) =
1398 // diff(-y1^2 + (yi + ya)*y1 - ya*yi)/diff(y1) =
1399 // -2*y1 + yi + ya
1400
1401 // diff(((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)*(xi - x1)^2)/diff(y1) =
1402 // 2*(y1 - ya)*(xi - x1)^2
1403
1404 // diff(((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)*(yi - y1)^2)/diff(y1) =
1405 // -2*(y1 - ya)*(yi - y1)^2 -2*(yi - y1)*((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)
1406 // -2*((y1 - ya)*(yi - y1)^2 + (yi - y1)*((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2))
1407
1408 // diff(((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)*(zi - z1)^2)/diff(y1) =
1409 // 2*(y1 - ya)*(zi - z1)^2
1410
1411 // diff((x1 - xa)*(y1 - ya)*(xi - x1)*(yi - y1))/diff(y1) =
1412 // (x1 - xa)*(xi - x1)*(yi - y1) - (xi - x1)*(x1 - xa)*(y1 - ya) =
1413 // (x1 - xa)*((xi - x1)*(yi - y1) - (xi - x1)*(y1 - ya)) =
1414 // (x1 - xa)*(xi - x1)*(-2*y1 + yi + ya)
1415
1416 // diff((y1 - ya)*(z1 - za)*(yi - y1)*(zi - z1))/diff(y1) =
1417 // (z1 - za)*(yi - y1)*(zi - z1) - (zi - z1)*(y1 - ya)*(z1 - za) =
1418 // (z1 - za)*((yi - y1)*(zi - z1) - (zi - z1)*(y1 - ya)) =
1419 // (z1 - za)*(zi - z1)*(-2*y1 + yi + ya)
1420
1421 // diff((x1 - xa)*(z1 - za)*(xi - x1)*(zi - z1))/diff(y1) = 0
1422
1423 // diff(Pr(pi))/diff(y1) = -10*n/ln(10)*((-2*y1 + yi + ya)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2) - ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))*2*(y1 - ya))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2
1424 // -5*n/ln(10)*(2*(y1 - ya)*(xi - x1)^2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - ((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)*(xi - x1)^2*2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*2*(y1 - ya))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1425 // -5*n/ln(10)*(-2*((y1 - ya)*(yi - y1)^2 + (yi - y1)*((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2))*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - ((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)*(yi - y1)^2*2((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*2(y1 -ya))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1426 // -5*n/ln(10)*(2*(y1 - ya)*(zi - z1)^2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - ((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)*(zi - z1)^2*2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*2*(y1 - ya))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1427 // +20*n/ln(10)*((x1 - xa)*(xi - x1)*(-2*y1 + yi + ya)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - (x1 - xa)*(y1 - ya)*(xi - x1)*(yi - y1)*2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*2*(y1 - ya))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1428 // +20*n/ln(10)*((z1 - za)*(zi - z1)*(-2*y1 + yi + ya)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - (y1 - ya)*(z1 - za)*(yi - y1)*(zi - z1)*2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*2*(y1 - ya))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1429 // +20*n/ln(10)*(0*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - (x1 - xa)*(z1 - za)*(xi - x1)*(zi - z1)*2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*2*(y1 - ya))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1430
1431 // diff(Pr(pi))/diff(y1) = -10*n/ln(10)*((-2*y1 + yi + ya)*d1a^2 - ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))*2*(y1 - ya))/d1a^4
1432 // -5*n/ln(10)*(2*(y1 - ya)*(xi - x1)^2*d1a^4 - (((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)*(xi - x1)^2)*4*d1a^2*(y1 - ya))/d1a^8
1433 // -5*n/ln(10)*(-2*((y1 - ya)*(yi - y1)^2 + (yi - y1)*((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2))*d1a^4 - (((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)*(yi - y1)^2)*4*d1a^2*(y1 -ya))/d1a^8
1434 // -5*n/ln(10)*(2*(y1 - ya)*(zi - z1)^2*d1a^4 - (((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)*(zi - z1)^2)*4*d1a^2*(y1 - ya))/d1a^8
1435 // +20*n/ln(10)*((x1 - xa)*(xi - x1)*(-2*y1 + yi + ya)*d1a^4 - (x1 - xa)*(y1 - ya)*(xi - x1)*(yi - y1)*4*d1a^2*(y1 - ya))/d1a^8
1436 // +20*n/ln(10)*((z1 - za)*(zi - z1)*(-2*y1 + yi + ya)*d1a^4 - (y1 - ya)*(z1 - za)*(yi - y1)*(zi - z1)*4*d1a^2*(y1 - ya))/d1a^8
1437 // +20*n/ln(10)*(-(x1 - xa)*(z1 - za)*(xi - x1)*(zi - z1)*4*d1a^2*(y1 - ya))/d1a^8
1438 final var tmp4 = diffY1a * diffZ1a * diffYi1 * diffZi1 * 4.0 * d1a2 * diffY1a;
1439 final var derivativeY1 = -10.0 * pathLossExponent / ln10 * ((-2.0 * y1 + yi + ya) * d1a2 - crossDiff * 2.0 * diffY1a) / d1a4
1440 - 5.0 * pathLossExponent / ln10 * (2.0 * diffY1a * diffXi12 * d1a4 - (-diffX1a2 + diffY1a2 + diffZ1a2) * diffXi12 * 4.0 * d1a2 * diffY1a) / d1a8
1441 - 5.0 * pathLossExponent / ln10 * (-2.0 * (diffY1a * diffYi12 + diffYi1 * (diffX1a2 - diffY1a2 + diffZ1a2)) * d1a4 - (diffX1a2 - diffY1a2 + diffZ1a2) * diffYi12 * 4.0 * d1a2 * diffY1a) / d1a8
1442 - 5.0 * pathLossExponent / ln10 * (2.0 * diffY1a * diffZi12 * d1a4 - (diffX1a2 + diffY1a2 - diffZ1a2) * diffZi12 * 4.0 * d1a2 * diffY1a) / d1a8
1443 + 20.0 * pathLossExponent / ln10 * (diffX1a * diffXi1 * (-2.0 * y1 + yi + ya) * d1a4 - diffX1a * diffY1a * diffXi1 * diffYi1 * 4.0 * d1a2 * diffY1a) / d1a8
1444 + 20.0 * pathLossExponent / ln10 * (diffZ1a * diffZi1 * (-2.0 * y1 + yi + ya) * d1a4 - tmp4) / d1a8
1445 + 20.0 * pathLossExponent / ln10 * (-diffX1a * diffZ1a * diffXi1 * diffZi1 * 4.0 * d1a2 * diffY1a) / d1a8;
1446
1447 // derivative of rssi respect to z1
1448
1449 // We have
1450 // Pr(pi) = Pr(p1)
1451 // -10*n*((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2))
1452 // -5*n*((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(xi - x1)^2
1453 // -5*n*((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(yi - y1)^2
1454 // -5*n*((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(zi - z1)^2
1455 // +20*n*(x1 - xa)*(y1 - ya)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(xi - x1)*(yi - y1)
1456 // +20*n*(y1 - ya)*(z1 - za)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(yi - y1)*(zi - z1)
1457 // +20*n*(x1 - xa)*(z1 - za)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(xi - x1)*(zi - z1)
1458
1459 // Hence
1460 // diff(((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1)))/diff(z1) =
1461 // diff(z1*zi -za*zi -z1^2 + za*z1)/diff(z1) =
1462 // diff(-z1^2 + (zi + za)*z1 - za*zi)/diff(z1) =
1463 // -2*z1 + zi + za
1464
1465 // diff(((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)*(xi - x1)^2)/diff(z1) =
1466 // 2*(z1 - za)*(xi - x1)^2
1467
1468 // diff(((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)*(yi - y1)^2)/diff(z1) =
1469 // 2*(z1 - za)*(yi - y1)^2
1470
1471 // diff(((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)*(zi - z1)^2)/diff(z1) =
1472 // 2*(z1 - za)*(zi - z1)^2 + 2*(zi - z1)*((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)
1473 // 2*((z1 - za)*(zi - z1)^2 + (zi - z1)*((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2))
1474
1475 // diff((x1 - xa)*(y1 - ya)*(xi - x1)*(yi - y1))/diff(z1) = 0
1476
1477 // diff((y1 - ya)*(z1 - za)*(yi - y1)*(zi - z1))/diff(z1) =
1478 // (y1 - ya)*(yi - y1)*(zi - z1) - (yi - y1)*(y1 - ya)*(z1 - za) =
1479 // (y1 - ya)*((yi - y1)*(zi - z1) - (yi - y1)*(z1 - za)) =
1480 // (y1 - ya)*(yi - y1)*(-2*z1 + zi + za)
1481
1482 // diff((x1 - xa)*(z1 - za)*(xi - x1)*(zi - z1))/diff(z1) =
1483 // (x1 - xa)*(xi - x1)*(zi - z1) - (xi - x1)*(x1 - xa)*(z1 - za) =
1484 // (x1 - xa)*((xi - x1)*(zi - z1) - (xi - x1)*(z1 - za)) =
1485 // (x1 - xa)*(xi - x1)*(-2*z1 + zi + za)
1486
1487 // diff(Pr(pi))/diff(z1) = -10*n/ln(10)*((-2*z1 + zi + za)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2) - ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))*2*(z1 - za))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2
1488 // -5*n/ln(10)*(2*(z1 - za)*(xi - x1)^2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - (((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)*(xi - x1)^2)*2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*2*(z1 - za))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1489 // -5*n/ln(10)*(2*(z1 - za)*(yi - y1)^2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - (((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)*(yi - y1)^2)*2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*2*(z1 - za))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1490 // -5*n/ln(10)*(2*((z1 - za)*(zi - z1)^2 + (zi - z1)*((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2))*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - (((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)*(zi - z1)^2)*2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*2*(z1 - za))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1491 // +20*n/ln(10)*(0*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2 - ((x1 - xa)*(y1 - ya)*(xi - x1)*(yi - y1))*2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*2*(z1 - za))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1492 // +20*n/ln(10)*((y1 - ya)*(yi - y1)*(-2*z1 + zi + za)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - ((y1 - ya)*(z1 - za)*(yi - y1)*(zi - z1))*2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*2*(z1 - za))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1493 // +20*n/ln(10)*((x1 - xa)*(xi - x1)*(-2*z1 + zi + za)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - ((x1 - xa)*(z1 - za)*(xi - x1)*(zi - z1))*2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*2*(z1 - za))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1494
1495 // diff(Pr(pi))/diff(z1) = -10*n/ln(10)*((-2*z1 + zi + za)*d1a^2 - ((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))*2*(z1 - za))/d1a^4
1496 // -5*n/ln(10)*(2*(z1 - za)*(xi - x1)^2*d1a^4 - (((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)*(xi - x1)^2)*4*d1a^2*(z1 - za))/d1a^8
1497 // -5*n/ln(10)*(2*(z1 - za)*(yi - y1)^2*d1a^4 - (((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)*(yi - y1)^2)*4*d1a^2*(z1 - za))/d1a^8
1498 // -5*n/ln(10)*(2*((z1 - za)*(zi - z1)^2 + (zi - z1)*((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2))*d1a^4 - (((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)*(zi - z1)^2)*4*d1a^2*(z1 - za))/d1a^8
1499 // +20*n/ln(10)*(-(x1 - xa)*(y1 - ya)*(xi - x1)*(yi - y1)*4*d1a^2*(z1 - za))/d1a^8
1500 // +20*n/ln(10)*((y1 - ya)*(yi - y1)*(-2*z1 + zi + za)*d1a^4 - ((y1 - ya)*(z1 - za)*(yi - y1)*(zi - z1))*4*d1a^2*(z1 - za))/d1a^8
1501 // +20*n/ln(10)*((x1 - xa)*(xi - x1)*(-2*z1 + zi + za)*d1a^4 - ((x1 - xa)*(z1 - za)*(xi - x1)*(zi - z1))*4*d1a^2*(z1 - za))/d1a^8
1502 final var derivativeZ1 = -10.0 * pathLossExponent / ln10 * ((-2.0 * z1 + zi + za) * d1a2 - crossDiff * 2.0 * diffZ1a) / d1a4
1503 - 5.0 * pathLossExponent / ln10 * (2.0 * diffZ1a * diffXi12 * d1a4 - ((-diffX1a2 + diffY1a2 + diffZ1a2) * diffXi12) * 4.0 * d1a2 * diffZ1a) / d1a8
1504 - 5.0 * pathLossExponent / ln10 * (2.0 * diffZ1a * diffYi12 * d1a4 - ((diffX1a2 - diffY1a2 + diffZ1a2) * diffYi12) * 4.0 * d1a2 * diffZ1a) / d1a8
1505 - 5.0 * pathLossExponent / ln10 * (2.0 * (diffZ1a * diffZi12 + diffZi1 * (diffX1a2 + diffY1a2 - diffZ1a2)) * d1a4 - ((diffX1a2 + diffY1a2 - diffZ1a2) * diffZi12) * 4.0 * d1a2 * diffZ1a) / d1a8
1506 + 20.0 * pathLossExponent / ln10 * (-diffX1a * diffY1a * diffXi1 * diffYi1 * 4.0 * d1a2 * diffZ1a) / d1a8
1507 + 20.0 * pathLossExponent / ln10 * (diffY1a * diffYi1 * (-2.0 * z1 + zi + za) * d1a4 - diffY1a * diffZ1a * diffYi1 * diffZi1 * 4.0 * d1a2 * diffZ1a) / d1a8
1508 + 20.0 * pathLossExponent / ln10 * (diffX1a * diffXi1 * (-2.0 * z1 + zi + za) * d1a4 - diffX1a * diffZ1a * diffXi1 * diffZi1 * 4.0 * d1a2 * diffZ1a) / d1a8;
1509
1510 // derivative of rssi respect to xa
1511
1512 // We have
1513 // Pr(pi) = Pr(p1)
1514 // -10*n*((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2))
1515 // -5*n*((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(xi - x1)^2
1516 // -5*n*((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(yi - y1)^2
1517 // -5*n*((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(zi - z1)^2
1518 // +20*n*(x1 - xa)*(y1 - ya)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(xi - x1)*(yi - y1)
1519 // +20*n*(y1 - ya)*(z1 - za)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(yi - y1)*(zi - z1)
1520 // +20*n*(x1 - xa)*(z1 - za)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(xi - x1)*(zi - z1)
1521
1522 // Hence
1523 // diff(((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1)))/diff(xa) =
1524 // -(xi - x1)
1525
1526 // diff(((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)*(xi - x1)^2)/diff(xa) =
1527 // 2*(x1 - xa)*(xi - x1)^2
1528
1529 // diff(((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)*(yi - y1)^2)/diff(xa) =
1530 // -2*(x1 - xa)*(yi - y1)^2
1531
1532 // diff(((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)*(zi - z1)^2)/diff(xa) =
1533 // -2*(x1 - xa)*(zi - z1)^2
1534
1535 // diff((x1 - xa)*(y1 - ya)*(xi - x1)*(yi - y1))/diff(xa) =
1536 // -(y1 - ya)*(xi - x1)*(yi - y1)
1537
1538 // diff((y1 - ya)*(z1 - za)*(yi - y1)*(zi - z1))/diff(xa) = 0
1539
1540 // diff((x1 - xa)*(z1 - za)*(xi - x1)*(zi - z1))/diff(xa) =
1541 // -(z1 - za)*(xi - x1)*(zi - z1)
1542
1543 // diff(Pr(pi))/diff(xa) = -10*n/ln(10)*(-(xi - x1)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2) - (((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1)))*-2*(x1 - xa))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2
1544 // -5*n/ln(10)*(2*(x1 - xa)*(xi - x1)^2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - (((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)*(xi - x1)^2)*2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*-2*(x1 - xa))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1545 // -5*n/ln(10)*(-2*(x1 - xa)*(yi - y1)^2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - (((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)*(yi - y1)^2)*2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*-2*(x1 - xa))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1546 // -5*n/ln(10)*(-2*(x1 - xa)*(zi - z1)^2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - (((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)*(zi - z1)^2)*2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*-2*(x1 - xa))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1547 // +20*n/ln(10)*(-(y1 - ya)*(xi - x1)*(yi - y1)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - (x1 - xa)*(y1 - ya)*(xi - x1)*(yi - y1)*2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*-2*(x1 - xa))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1548 // +20*n/ln(10)*(0*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - (y1 - ya)*(z1 - za)*(yi - y1)*(zi - z1)*2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*-2*(x1 - xa))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1549 // +20*n/ln(10)*(-(z1 - za)*(xi - x1)*(zi - z1)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - (x1 - xa)*(z1 - za)*(xi - x1)*(zi - z1)*2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*-2*(x1 - xa))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1550
1551 // diff(Pr(pi))/diff(xa) = -10*n/ln(10)*(-(xi - x1)*d1a^2 + (((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1)))*2*(x1 - xa))/d1a^4
1552 // -5*n/ln(10)*(2*(x1 - xa)*(xi - x1)^2*d1a^4 + (((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)*(xi - x1)^2)*4*d1a^2*(x1 - xa))/d1a^8
1553 // -5*n/ln(10)*(-2*(x1 - xa)*(yi - y1)^2*d1a^4 + (((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)*(yi - y1)^2)*4*d1a^2*(x1 - xa))/d1a^8
1554 // -5*n/ln(10)*(-2*(x1 - xa)*(zi - z1)^2*d1a^4 + (((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)*(zi - z1)^2)*4*d1a^2*(x1 - xa))/d1a^8
1555 // +20*n/ln(10)*(-(y1 - ya)*(xi - x1)*(yi - y1)*d1a^4 + (x1 - xa)*(y1 - ya)*(xi - x1)*(yi - y1)*4*d1a^2*(x1 - xa))/d1a^8
1556 // +20*n/ln(10)*((y1 - ya)*(z1 - za)*(yi - y1)*(zi - z1)*4*d1a^2*(x1 - xa))/d1a^8
1557 // +20*n/ln(10)*(-(z1 - za)*(xi - x1)*(zi - z1)*d1a^4 + (x1 - xa)*(z1 - za)*(xi - x1)*(zi - z1)*4*d1a^2*(x1 - xa))/d1a^8
1558 final var derivativeXa = -10.0 * pathLossExponent / ln10 * (-diffXi1 * d1a2 + crossDiff * 2.0 * diffX1a) / d1a4
1559 - 5.0 * pathLossExponent / ln10 * (2.0 * diffX1a * diffXi12 * d1a4 + ((diffY1a2 + diffZ1a2 - diffX1a2) * diffXi12) * 4.0 * d1a2 * diffX1a) / d1a8
1560 - 5.0 * pathLossExponent / ln10 * (-2.0 * diffX1a * diffYi12 * d1a4 + ((diffX1a2 - diffY1a2 + diffZ1a2) * diffYi12) * 4.0 * d1a2 * diffX1a) / d1a8
1561 - 5.0 * pathLossExponent / ln10 * (-2.0 * diffX1a * diffZi12 * d1a4 + ((diffX1a2 + diffY1a2 - diffZ1a2) * diffZi12) * 4.0 * d1a2 * diffX1a) / d1a8
1562 + 20.0 * pathLossExponent / ln10 * (-diffY1a * diffXi1 * diffYi1 * d1a4 + tmp2) / d1a8
1563 + 20.0 * pathLossExponent / ln10 * (diffY1a * diffZ1a * diffYi1 * diffZi1 * 4.0 * d1a2 * diffX1a) / d1a8
1564 + 20.0 * pathLossExponent / ln10 * (-diffZ1a * diffXi1 * diffZi1 * d1a4 + tmp3) / d1a8;
1565
1566 // derivative of rssi respect to ya
1567
1568 // We have
1569 // Pr(pi) = Pr(p1)
1570 // -10*n*((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2))
1571 // -5*n*((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(xi - x1)^2
1572 // -5*n*((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(yi - y1)^2
1573 // -5*n*((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(zi - z1)^2
1574 // +20*n*(x1 - xa)*(y1 - ya)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(xi - x1)*(yi - y1)
1575 // +20*n*(y1 - ya)*(z1 - za)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(yi - y1)*(zi - z1)
1576 // +20*n*(x1 - xa)*(z1 - za)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(xi - x1)*(zi - z1)
1577
1578 // Hence
1579 // diff(((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1)))/diff(ya) =
1580 // -(yi - y1)
1581
1582 // diff(((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)*(xi - x1)^2)/diff(ya) =
1583 // -2*(y1 - ya)*(xi - x1)^2
1584
1585 // diff(((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)*(yi - y1)^2)/diff(ya) =
1586 // 2*(y1 - ya)*(yi - y1)^2
1587
1588 // diff(((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)*(zi - z1)^2)/diff(ya) =
1589 // -2*(y1 - ya)*(zi - z1)^2
1590
1591 // diff((x1 - xa)*(y1 - ya)*(xi - x1)*(yi - y1))/diff(ya) =
1592 // -(x1 - xa)*(xi - x1)*(yi - y1)
1593
1594 // diff((y1 - ya)*(z1 - za)*(yi - y1)*(zi - z1))/diff(ya) =
1595 // -(z1 - za)*(yi - y1)*(zi - z1)
1596
1597 // diff((x1 - xa)*(z1 - za)*(xi - x1)*(zi - z1))/diff(ya) = 0
1598
1599 // diff(Pr(pi))/diff(ya) = -10*n/ln(10)*(-(yi - y1)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2) - (((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1)))*-2*(y1 - ya))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2
1600 // -5*n/ln(10)*(-2*(y1 - ya)*(xi - x1)^2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - (((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)*(xi - x1)^2)*2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*-2*(y1 - ya))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1601 // -5*n/ln(10)*(2*(y1 - ya)*(yi - y1)^2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - (((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)*(yi - y1)^2)*2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*-2*(y1 - ya))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1602 // -5*n/ln(10)*(-2*(y1 - ya)*(zi - z1)^2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - (((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)*(zi - z1)^2)*2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*-2*(y1 - ya))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1603 // +20*n/ln(10)*(-(x1 - xa)*(xi - x1)*(yi - y1)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - ((x1 - xa)*(y1 - ya)*(xi - x1)*(yi - y1))*2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*-2*(y1 - ya))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1604 // +20*n/ln(10)*(-(z1 - za)*(yi - y1)*(zi - z1)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - (y1 - ya)*(z1 - za)*(yi - y1)*(zi - z1)*2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*-2*(y1 - ya))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1605 // +20*n/ln(10)*(0*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - ((x1 - xa)*(z1 - za)*(xi - x1)*(zi - z1))*2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*-2*(y1 - ya))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1606
1607 // diff(Pr(pi))/diff(ya) = -10*n/ln(10)*(-(yi - y1)*d1a^2 + (((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1)))*2*(y1 - ya))/d1a^4
1608 // -5*n/ln(10)*(-2*(y1 - ya)*(xi - x1)^2*d1a^4 + (((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)*(xi - x1)^2)*4*d1a^2*(y1 - ya))/d1a^8
1609 // -5*n/ln(10)*(2*(y1 - ya)*(yi - y1)^2*d1a^4 + (((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)*(yi - y1)^2)*4*d1a^2*(y1 - ya))/d1a^8
1610 // -5*n/ln(10)*(-2*(y1 - ya)*(zi - z1)^2*d1a^4 + (((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)*(zi - z1)^2)*4*d1a^2*(y1 - ya))/d1a^8
1611 // +20*n/ln(10)*(-(x1 - xa)*(xi - x1)*(yi - y1)*d1a^4 + ((x1 - xa)*(y1 - ya)*(xi - x1)*(yi - y1))*4*d1a^2*(y1 - ya))/d1a^8
1612 // +20*n/ln(10)*(-(z1 - za)*(yi - y1)*(zi - z1)*d1a^4 + (y1 - ya)*(z1 - za)*(yi - y1)*(zi - z1)*4*d1a^2*(y1 - ya))/d1a^8
1613 // +20*n/ln(10)*(((x1 - xa)*(z1 - za)*(xi - x1)*(zi - z1))*4*d1a^2*(y1 - ya))/d1a^8
1614 final var derivativeYa = -10.0 * pathLossExponent / ln10 * (-diffYi1 * d1a2 + crossDiff * 2.0 * diffY1a) / d1a4
1615 - 5.0 * pathLossExponent / ln10 * (-2.0 * diffY1a * diffXi12 * d1a4 + ((diffY1a2 + diffZ1a2 - diffX1a2) * diffXi12) * 4.0 * d1a2 * diffY1a) / d1a8
1616 - 5.0 * pathLossExponent / ln10 * (2.0 * diffY1a * diffYi12 * d1a4 + ((diffX1a2 - diffY1a2 + diffZ1a2) * diffYi12) * 4.0 * d1a2 * diffY1a) / d1a8
1617 - 5.0 * pathLossExponent / ln10 * (-2.0 * diffY1a * diffZi12 * d1a4 + ((diffX1a2 + diffY1a2 - diffZ1a2) * diffZi12) * 4.0 * d1a2 * diffY1a) / d1a8
1618 + 20.0 * pathLossExponent / ln10 * (-diffX1a * diffXi1 * diffYi1 * d1a4 + (diffX1a * diffY1a * diffXi1 * diffYi1) * 4.0 * d1a2 * diffY1a) / d1a8
1619 + 20.0 * pathLossExponent / ln10 * (-diffZ1a * diffYi1 * diffZi1 * d1a4 + tmp4) / d1a8
1620 + 20.0 * pathLossExponent / ln10 * ((diffX1a * diffZ1a * diffXi1 * diffZi1) * 4.0 * d1a2 * diffY1a) / d1a8;
1621
1622 // derivative of rssi respect to za
1623
1624 // We have
1625 // Pr(pi) = Pr(p1)
1626 // -10*n*((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2))
1627 // -5*n*((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(xi - x1)^2
1628 // -5*n*((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(yi - y1)^2
1629 // -5*n*((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(zi - z1)^2
1630 // +20*n*(x1 - xa)*(y1 - ya)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(xi - x1)*(yi - y1)
1631 // +20*n*(y1 - ya)*(z1 - za)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(yi - y1)*(zi - z1)
1632 // +20*n*(x1 - xa)*(z1 - za)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(xi - x1)*(zi - z1)
1633
1634 // Hence
1635 // diff(((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1)))/diff(za) =
1636 // -(zi - z1)
1637
1638 // diff(((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)*(xi - x1)^2)/diff(za) =
1639 // -2*(z1 - za)*(xi - x1)^2
1640
1641 // diff(((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)*(yi - y1)^2)/diff(za) =
1642 // -2*(z1 - za)*(yi - y1)^2
1643
1644 // diff(((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)*(zi - z1)^2)/diff(za) =
1645 // 2*(z1 - za)*(zi - z1)^2
1646
1647 // diff((x1 - xa)*(y1 - ya)*(xi - x1)*(yi - y1))/diff(za) = 0
1648
1649 // diff((y1 - ya)*(z1 - za)*(yi - y1)*(zi - z1))/diff(za) =
1650 // -(y1 - ya)*(yi - y1)*(zi - z1)
1651
1652 // diff((x1 - xa)*(z1 - za)*(xi - x1)*(zi - z1))/diff(za) =
1653 // -(x1 - xa)*(xi - x1)*(zi - z1)
1654
1655 // diff(Pr(pi))/diff(za) = -10*n/ln(10)*(-(zi - z1)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2) - (((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1)))*-2*(z1 - za))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2
1656 // -5*n/ln(10)*(-2*(z1 - za)*(xi - x1)^2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - (((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)*(xi - x1)^2)*2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*-2*(z1 - za))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1657 // -5*n/ln(10)*(-2*(z1 - za)*(yi - y1)^2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - (((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)*(yi - y1)^2)*2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*-2*(z1 - za))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1658 // -5*n/ln(10)*(2*(z1 - za)*(zi - z1)^2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - (((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)*(zi - z1)^2)*2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*-2*(z1 - za))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1659 // +20*n/ln(10)*(0*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - ((x1 - xa)*(y1 - ya)*(xi - x1)*(yi - y1))*2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*-2*(z1 - za))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1660 // +20*n/ln(10)*(-(y1 - ya)*(yi - y1)*(zi - z1)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - ((y1 - ya)*(z1 - za)*(yi - y1)*(zi - z1))*2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*-2*(z1 - za))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1661 // +20*n/ln(10)*(-(x1 - xa)*(xi - x1)*(zi - z1)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - ((x1 - xa)*(z1 - za)*(xi - x1)*(zi - z1))*2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*-2*(z1 - za))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1662
1663 // diff(Pr(pi))/diff(za) = -10*n/ln(10)*(-(zi - z1)*d1a^2 + (((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1)))*2*(z1 - za))/d1a^4
1664 // -5*n/ln(10)*(-2*(z1 - za)*(xi - x1)^2*d1a^4 - (((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)*(xi - x1)^2)*4*d1a^2*(z1 - za))/d1a^8
1665 // -5*n/ln(10)*(-2*(z1 - za)*(yi - y1)^2*d1a^4 + (((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)*(yi - y1)^2)*4*d1a^2*(z1 - za))/d1a^8
1666 // -5*n/ln(10)*(2*(z1 - za)*(zi - z1)^2*d1a^4 + (((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)*(zi - z1)^2)*4*d1a^2*(z1 - za))/d1a^8
1667 // +20*n/ln(10)*(((x1 - xa)*(y1 - ya)*(xi - x1)*(yi - y1))*4*d1a^2*(z1 - za))/d1a^8
1668 // +20*n/ln(10)*(-(y1 - ya)*(yi - y1)*(zi - z1)*d1a^4 + ((y1 - ya)*(z1 - za)*(yi - y1)*(zi - z1))*4*d1a^2*(z1 - za))/d1a^8
1669 // +20*n/ln(10)*(-(x1 - xa)*(xi - x1)*(zi - z1)*d1a^4 + ((x1 - xa)*(z1 - za)*(xi - x1)*(zi - z1))*4*d1a^2*(z1 - za))/d1a^8
1670 final var derivativeZa = -10.0 * pathLossExponent / ln10 * (-diffZi1 * d1a2 + crossDiff * 2.0 * diffZ1a) / d1a4
1671 - 5.0 * pathLossExponent / ln10 * (-2.0 * diffZ1a * diffXi12 * d1a4 - (-diffX1a2 + diffY1a2 + diffZ1a2) * diffXi12 * 4.0 * d1a2 * diffZ1a) / d1a8
1672 - 5.0 * pathLossExponent / ln10 * (-2.0 * diffZ1a * diffYi12 * d1a4 + (diffX1a2 - diffY1a2 + diffZ1a2) * diffYi12 * 4.0 * d1a2 * diffZ1a) / d1a8
1673 - 5.0 * pathLossExponent / ln10 * (2.0 * diffZ1a * diffZi12 * d1a4 + (diffX1a2 + diffY1a2 - diffZ1a2) * diffZi12 * 4.0 * d1a2 * diffZ1a) / d1a8
1674 + 20.0 * pathLossExponent / ln10 * ((diffX1a * diffY1a * diffXi1 * diffYi1) * 4.0 * d1a2 * diffZ1a) / d1a8
1675 + 20.0 * pathLossExponent / ln10 * (-diffY1a * diffYi1 * diffZi1 * d1a4 + (diffY1a * diffZ1a * diffYi1 * diffZi1) * 4.0 * d1a2 * diffZ1a) / d1a8
1676 + 20.0 * pathLossExponent / ln10 * (-diffX1a * diffXi1 * diffZi1 * d1a4 + (diffX1a * diffZ1a * diffXi1 * diffZi1) * 4.0 * d1a2 * diffZ1a) / d1a8;
1677
1678 // derivative of rssi respect to xi
1679
1680 // We have
1681 // Pr(pi) = Pr(p1)
1682 // -10*n*((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2))
1683 // -5*n*((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(xi - x1)^2
1684 // -5*n*((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(yi - y1)^2
1685 // -5*n*((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(zi - z1)^2
1686 // +20*n*(x1 - xa)*(y1 - ya)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(xi - x1)*(yi - y1)
1687 // +20*n*(y1 - ya)*(z1 - za)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(yi - y1)*(zi - z1)
1688 // +20*n*(x1 - xa)*(z1 - za)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(xi - x1)*(zi - z1)
1689
1690 // Hence
1691 // diff(((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1)))/diff(xi) =
1692 // x1 - xa
1693
1694 // diff(((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)*(xi - x1)^2)/diff(xi) =
1695 // 2*(xi - x1)*((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)
1696
1697 // diff(((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)*(yi - y1)^2)/diff(xi) = 0
1698
1699 // diff(((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)*(zi - z1)^2)/diff(xi) = 0
1700
1701 // diff((x1 - xa)*(y1 - ya)*(xi - x1)*(yi - y1))/diff(xi) =
1702 // (x1 - xa)*(y1 - ya)*(yi - y1)
1703
1704 // diff((y1 - ya)*(z1 - za)*(yi - y1)*(zi - z1))/diff(xi) = 0
1705
1706 // diff((x1 - xa)*(z1 - za)*(xi - x1)*(zi - z1))/diff(xi) =
1707 // (x1 - xa)*(z1 - za)*(zi - z1)
1708
1709 // diff(Pr(pi))/diff(xi) = -10*n/ln(10)*((x1 - xa)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2) - (((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1)))*0)/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2
1710 // -5*n/ln(10)*(2*(xi - x1)*((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - (((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)*(xi - x1)^2)*0)/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1711 // -5*n/ln(10)*(0*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - (((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)*(yi - y1)^2)*0)/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1712 // -5*n/ln(10)*(0*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - (((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)*(zi - z1)^2)*0)/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1713 // +20*n/ln(10)*((x1 - xa)*(y1 - ya)*(yi - y1)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - ((x1 - xa)*(y1 - ya)*(xi - x1)*(yi - y1))*0)/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1714 // +20*n/ln(10)*(0*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - ((y1 - ya)*(z1 - za)*(yi - y1)*(zi - z1))*0)/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1715 // +20*n/ln(10)*((x1 - xa)*(z1 - za)*(zi - z1)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - ((x1 - xa)*(z1 - za)*(xi - x1)*(zi - z1))*0)/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1716
1717 // diff(Pr(pi))/diff(xi) = -10*n/ln(10)*(x1 - xa)*d1a^2/d1a^4
1718 // -5*n/ln(10)*(2*(xi - x1)*((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)*d1a^4)/d1a^8
1719 // +20*n/ln(10)*(x1 - xa)*(y1 - ya)*(yi - y1)*d1a^4/d1a^8
1720 // +20*n/ln(10)*(x1 - xa)*(z1 - za)*(zi - z1)*d1a^4)/d1a^8
1721
1722 // diff(Pr(pi))/diff(xi) = -10*n/ln(10)*(x1 - xa)/d1a^2
1723 // -10*n/ln(10)*(xi - x1)*((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)/d1a^4
1724 // +20*n/ln(10)*(x1 - xa)*(y1 - ya)*(yi - y1)/d1a^4
1725 // +20*n/ln(10)*(x1 - xa)*(z1 - za)*(zi - z1)/d1a^4
1726 final var derivativeXi = -10.0 * pathLossExponent / ln10 * diffX1a / d1a2
1727 - 10.0 * pathLossExponent / ln10 * diffXi1 * (-diffX1a2 + diffY1a2 + diffZ1a2) / d1a4
1728 + 20.0 * pathLossExponent / ln10 * diffX1a * diffY1a * diffYi1 / d1a4
1729 + 20.0 * pathLossExponent / ln10 * diffX1a * diffZ1a * diffZi1 / d1a4;
1730
1731 // derivative of rssi respect to yi
1732
1733 // We have
1734 // Pr(pi) = Pr(p1)
1735 // -10*n*((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2))
1736 // -5*n*((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(xi - x1)^2
1737 // -5*n*((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(yi - y1)^2
1738 // -5*n*((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(zi - z1)^2
1739 // +20*n*(x1 - xa)*(y1 - ya)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(xi - x1)*(yi - y1)
1740 // +20*n*(y1 - ya)*(z1 - za)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(yi - y1)*(zi - z1)
1741 // +20*n*(x1 - xa)*(z1 - za)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(xi - x1)*(zi - z1)
1742
1743 // Hence
1744 // diff(((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1)))/diff(yi) =
1745 // y1 - ya
1746
1747 // diff(((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)*(xi - x1)^2)/diff(yi) = 0
1748
1749 // diff(((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)*(yi - y1)^2)/diff(yi) =
1750 // 2*(yi - y1)*((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)
1751
1752 // diff(((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)*(zi - z1)^2)/diff(yi) = 0
1753
1754 // diff((x1 - xa)*(y1 - ya)*(xi - x1)*(yi - y1))/diff(yi) =
1755 // (x1 - xa)*(y1 - ya)*(xi - x1)
1756
1757 // diff((y1 - ya)*(z1 - za)*(yi - y1)*(zi - z1))/diff(yi) =
1758 // (y1 - ya)*(z1 - za)*(zi - z1)
1759
1760 // diff((x1 - xa)*(z1 - za)*(xi - x1)*(zi - z1))/diff(yi) = 0
1761
1762 // diff(Pr(pi))/diff(yi) = -10*n/ln(10)*((y1 - ya)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2) - (((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1)))*0)/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2
1763 // -5*n/ln(10)*(0*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - (((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)*(xi - x1)^2)*0)/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1764 // -5*n/ln(10)*(2*(yi - y1)*((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - (((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)*(yi - y1)^2)*0)/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1765 // -5*n/ln(10)*(0*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - (((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)*(zi - z1)^2)*0)/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1766 // +20*n/ln(10)*((x1 - xa)*(y1 - ya)*(xi - x1)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - ((x1 - xa)*(y1 - ya)*(xi - x1)*(yi - y1))*0)/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1767 // +20*n/ln(10)*((y1 - ya)*(z1 - za)*(zi - z1)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - ((y1 - ya)*(z1 - za)*(yi - y1)*(zi - z1))*0)/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1768 // +20*n/ln(10)*(0*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - ((x1 - xa)*(z1 - za)*(xi - x1)*(zi - z1))*0)/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1769
1770 // diff(Pr(pi))/diff(yi) = -10*n/ln(10)*(y1 - ya)*d1a^2/d1a^4
1771 // -5*n/ln(10)*(2*(yi - y1)*((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)*d1a^4)/d1a^8
1772 // +20*n/ln(10)*((x1 - xa)*(y1 - ya)*(xi - x1)*d1a^4)/d1a^8
1773 // +20*n/ln(10)*((y1 - ya)*(z1 - za)*(zi - z1)*d1a^4)/d1a^8
1774
1775 // diff(Pr(pi))/diff(yi) = -10*n/ln(10)*(y1 - ya)/d1a^2
1776 // -10*n/ln(10)*(yi - y1)*((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)/d1a^4
1777 // +20*n/ln(10)*(x1 - xa)*(y1 - ya)*(xi - x1)/d1a^4
1778 // +20*n/ln(10)*(y1 - ya)*(z1 - za)*(zi - z1)/d1a^4
1779 final var derivativeYi = -10.0 * pathLossExponent / ln10 * diffY1a / d1a2
1780 - 10.0 * pathLossExponent / ln10 * diffYi1 * (diffX1a2 - diffY1a2 + diffZ1a2) / d1a4
1781 + 20.0 * pathLossExponent / ln10 * diffX1a * diffY1a * diffXi1 / d1a4
1782 + 20.0 * pathLossExponent / ln10 * diffY1a * diffZ1a * diffZi1 / d1a4;
1783
1784 // derivative of rssi respect to zi
1785
1786 // We have
1787 // Pr(pi) = Pr(p1)
1788 // -10*n*((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1))/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2))
1789 // -5*n*((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(xi - x1)^2
1790 // -5*n*((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(yi - y1)^2
1791 // -5*n*((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(zi - z1)^2
1792 // +20*n*(x1 - xa)*(y1 - ya)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(xi - x1)*(yi - y1)
1793 // +20*n*(y1 - ya)*(z1 - za)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(yi - y1)*(zi - z1)
1794 // +20*n*(x1 - xa)*(z1 - za)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)*(xi - x1)*(zi - z1)
1795
1796 // Hence
1797 // diff(((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1)))/diff(zi) =
1798 // z1 - za
1799
1800 // diff(((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)*(xi - x1)^2)/diff(zi) = 0
1801
1802 // diff(((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)*(yi - y1)^2)/diff(zi) = 0
1803
1804 // diff(((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)*(zi - z1)^2)/diff(zi) =
1805 // 2*(zi - z1)*((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)
1806
1807 // diff((x1 - xa)*(y1 - ya)*(xi - x1)*(yi - y1))/diff(zi) = 0
1808
1809 // diff((y1 - ya)*(z1 - za)*(yi - y1)*(zi - z1))/diff(zi) =
1810 // (y1 - ya)*(z1 - za)*(yi - y1)
1811
1812 // diff((x1 - xa)*(z1 - za)*(xi - x1)*(zi - z1))/diff(zi) =
1813 // (x1 - xa)*(z1 - za)*(xi - x1)
1814
1815 // diff(Pr(pi))/diff(zi) = -10*n/ln(10)*((z1 - za)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2) - (((x1 - xa)*(xi - x1) + (y1 - ya)*(yi - y1) + (z1 - za)*(zi - z1)))*0)/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2
1816 // -5*n/ln(10)*(0*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - (((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)*(xi - x1)^2)*0)/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1817 // -5*n/ln(10)*(0*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - (((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)*(yi - y1)^2)*0)/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1818 // -5*n/ln(10)*(2*(zi - z1)*((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - (((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)*(zi - z1)^2)*0)/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1819 // 20*n/ln(10)*(0*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - (x1 - xa)*(y1 - ya)*(xi - x1)*(yi - y1)*0)/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1820 // 20*n/ln(10)*((y1 - ya)*(z1 - za)*(yi - y1)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - ((y1 - ya)*(z1 - za)*(yi - y1)*(zi - z1))*0)/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1821 // 20*n/ln(10)*((x1 - xa)*(z1 - za)*(xi - x1)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - ((x1 - xa)*(z1 - za)*(xi - x1)*(zi - z1))*0)/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
1822
1823 // diff(Pr(pi))/diff(zi) = -10*n/ln(10)*((z1 - za)*d1a^2)/d1a^4
1824 // -5*n/ln(10)*(2*(zi - z1)*((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)*d1a^4)/d1a^8
1825 // 20*n/ln(10)*((y1 - ya)*(z1 - za)*(yi - y1)*d1a^4)/d1a^8
1826 // 20*n/ln(10)*((x1 - xa)*(z1 - za)*(xi - x1)*d1a^4)/d1a^8
1827
1828 // diff(Pr(pi))/diff(zi) = -10*n/ln(10)*(z1 - za)/d1a^2
1829 // -10*n/ln(10)*(zi - z1)*((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)/d1a^4
1830 // 20*n/ln(10)*(y1 - ya)*(z1 - za)*(yi - y1)/d1a^4
1831 // 20*n/ln(10)*(x1 - xa)*(z1 - za)*(xi - x1)/d1a^4
1832 final var derivativeZi = -10.0 * pathLossExponent / ln10 * diffZ1a / d1a2
1833 - 10.0 * pathLossExponent / ln10 * diffZi1 * (diffX1a2 + diffY1a2 - diffZ1a2) / d1a4
1834 + 20.0 * pathLossExponent / ln10 * diffY1a * diffZ1a * diffYi1 / d1a4
1835 + 20.0 * pathLossExponent / ln10 * diffX1a * diffZ1a * diffXi1 / d1a4;
1836
1837 // set derivatives fingerprintRssi, pathLossExponent, x1, y1, z1, xa, ya, za, xi, yi, zi
1838 jacobian.setElementAtIndex(0, derivativeFingerprintRssi);
1839 jacobian.setElementAtIndex(1, derivativePathLossExponent);
1840 jacobian.setElementAtIndex(2, derivativeX1);
1841 jacobian.setElementAtIndex(3, derivativeY1);
1842 jacobian.setElementAtIndex(4, derivativeZ1);
1843 jacobian.setElementAtIndex(5, derivativeXa);
1844 jacobian.setElementAtIndex(6, derivativeYa);
1845 jacobian.setElementAtIndex(7, derivativeZa);
1846 jacobian.setElementAtIndex(8, derivativeXi);
1847 jacobian.setElementAtIndex(9, derivativeYi);
1848 jacobian.setElementAtIndex(10, derivativeZi);
1849 }
1850
1851 @Override
1852 public int getNumberOfVariables() {
1853 return 1;
1854 }
1855 }, mean, covariance);
1856 } catch (final AlgebraException | StatisticsException e) {
1857 throw new IndoorException(e);
1858 }
1859 }
1860
1861 /**
1862 * Propagates provided variances (fingerprint rssi variance, path-loss exponent variance,
1863 * fingerprint position covariance and radio source position covariance) into
1864 * rssi variance by considering the 2D 3rd order Taylor expression of received power.
1865 * Notice that any unknown variance is assumed to be zero.
1866 *
1867 * @param fingerprintRssi closest located fingerprint reading RSSI expressed in dBm's.
1868 * @param pathLossExponent path-loss exponent.
1869 * @param fingerprintPosition position of closest fingerprint.
1870 * @param radioSourcePosition radio source position associated to fingerprint reading.
1871 * @param estimatedPosition position to be estimated. Usually this is equal to the
1872 * initial position used by a non-linear algorithm.
1873 * @param fingerprintRssiVariance variance of fingerprint RSSI or null if unknown.
1874 * @param pathLossExponentVariance variance of path-loss exponent or null if unknown.
1875 * @param fingerprintPositionCovariance covariance of fingerprint position or null if
1876 * unknown.
1877 * @param radioSourcePositionCovariance covariance of radio source position or null
1878 * if unknown.
1879 * @param estimatedPositionCovariance covariance of position to be estimated or null
1880 * if unknown. (This is usually unknown).
1881 * @return a normal distribution containing expected received RSSI value and its variance.
1882 * @throws IndoorException if something fails.
1883 */
1884 public static MultivariateNormalDist propagateVariancesToRssiVarianceThirdOrderNonLinear2D(
1885 final double fingerprintRssi, final double pathLossExponent,
1886 final Point2D fingerprintPosition, final Point2D radioSourcePosition,
1887 final Point2D estimatedPosition, final Double fingerprintRssiVariance,
1888 final Double pathLossExponentVariance,
1889 final Matrix fingerprintPositionCovariance,
1890 final Matrix radioSourcePositionCovariance,
1891 final Matrix estimatedPositionCovariance) throws IndoorException {
1892
1893 if (fingerprintPosition == null || radioSourcePosition == null || estimatedPosition == null) {
1894 return null;
1895 }
1896
1897 // 3rd order Taylor expression of received power in 2D:
1898 // Pr(pi) = Pr(p1)
1899 // -10*n*(x1 - xa)/(ln(10)*d1a^2)*(xi - x1) +
1900 // -10*n*(y1 - ya)/(ln(10)*d1a^2)*(yi - y1) +
1901 // -5*n*((y1 - ya)^2 - (x1 - xa)^2)/(ln(10)*d1a^4)*(xi - x1)^2 +
1902 // -5*n*((x1 - xa)^2 - (y1 - ya)^2)/(ln(10)*d1a^4)*(yi - y1)^2 +
1903 // 20*n*(x1 - xa)*(y1 - ya)/(ln(10)*d1a^4)*(xi - x1)*(yi - y1) +
1904 // -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 +
1905 // -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 +
1906 // -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) +
1907 // -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
1908 // where d1a2 = (x1 - xa)^2 + (y1 - ya)^2
1909
1910 final var x1 = fingerprintPosition.getInhomX();
1911 final var y1 = fingerprintPosition.getInhomY();
1912
1913 final var xa = radioSourcePosition.getInhomX();
1914 final var ya = radioSourcePosition.getInhomY();
1915
1916 final var xi = estimatedPosition.getInhomX();
1917 final var yi = estimatedPosition.getInhomY();
1918
1919 final var mean = new double[]{
1920 fingerprintRssi, pathLossExponent, x1, y1, xa, ya, xi, yi
1921 };
1922 final var covariance = Matrix.diagonal(new double[]{
1923 fingerprintRssiVariance != null ? fingerprintRssiVariance : 0.0,
1924 pathLossExponentVariance != null ? pathLossExponentVariance : 0.0,
1925 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
1926 });
1927
1928 if (fingerprintPositionCovariance != null
1929 && fingerprintPositionCovariance.getRows() == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH
1930 && fingerprintPositionCovariance.getColumns() == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH) {
1931
1932 covariance.setSubmatrix(2, 2, 3, 3,
1933 fingerprintPositionCovariance);
1934 }
1935
1936 if (radioSourcePositionCovariance != null
1937 && radioSourcePositionCovariance.getRows() == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH
1938 && radioSourcePositionCovariance.getColumns() == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH) {
1939 covariance.setSubmatrix(4, 4, 5, 5,
1940 radioSourcePositionCovariance);
1941 }
1942
1943 if (estimatedPositionCovariance != null
1944 && estimatedPositionCovariance.getRows() == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH
1945 && estimatedPositionCovariance.getColumns() == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH) {
1946 covariance.setSubmatrix(6, 6, 7, 7,
1947 estimatedPositionCovariance);
1948 }
1949
1950 try {
1951 // although less precise, we use a jacobian estimator to simplify expressions
1952 final MultiVariateFunctionEvaluatorListener evaluator = new MultiVariateFunctionEvaluatorListener() {
1953 @Override
1954 public void evaluate(final double[] point, final double[] result) {
1955 // Pr(pi) = Pr(p1)
1956 // -10*n*(x1 - xa)/(ln(10)*d1a^2)*(xi - x1)
1957 // -10*n*(y1 - ya)/(ln(10)*d1a^2)*(yi - y1)
1958 // -5*n*((y1 - ya)^2 - (x1 - xa)^2)/(ln(10)*d1a^4)*(xi - x1)^2
1959 // -5*n*((x1 - xa)^2 - (y1 - ya)^2)/(ln(10)*d1a^4)*(yi - y1)^2
1960 // +20*n*(x1 - xa)*(y1 - ya)/(ln(10)*d1a^4)*(xi - x1)*(yi - y1)
1961 // -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
1962 // -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
1963 // -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)
1964 // -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
1965 // where d1a2 = (x1 - xa)^2 + (y1 - ya)^2
1966
1967 // Hence:
1968 // Pr(pi) = Pr(p1)
1969 // -10*n*(x1 - xa)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2))*(xi - x1)
1970 // -10*n*(y1 - ya)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2))*(yi - y1)
1971 // -5*n*((y1 - ya)^2 - (x1 - xa)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2)^2)*(xi - x1)^2
1972 // -5*n*((x1 - xa)^2 - (y1 - ya)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2)^2)*(yi - y1)^2
1973 // +20*n*(x1 - xa)*(y1 - ya)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2)^2)*(xi - x1)*(yi - y1)
1974 // -10/6*n/ln(10)*(-2*(x1 - xa)*((x1 - xa)^2 + (y1 - ya)^2)^2 - ((y1 - ya)^2 - (x1 - xa)^2)*4*((x1 - xa)^2 + (y1 - ya)^2)*(x1 - xa))/((x1 - xa)^2 + (y1 - ya)^2)^4*(xi - x1)^3
1975 // -10/6*n/ln(10)*(-2*(y1 - ya)*((x1 - xa)^2 + (y1 - ya)^2)^2 - ((x1 - xa)^2 - (y1 - ya)^2)*4*((x1 - xa)^2 + (y1 - ya)^2)*(y1 - ya))/((x1 - xa)^2 + (y1 - ya)^2)^4*(yi - y1)^3
1976 // -5*n/ln(10)*(2*(y1 - ya)*((x1 - xa)^2 + (y1 - ya)^2)^2 - ((y1 - ya)^2 - (x1 - xa)^2)*4*((x1 - xa)^2 + (y1 - ya)^2)*(y1 - ya))/((x1 - xa)^2 + (y1 - ya)^2)^4*(xi - x1)^2*(yi - y1)
1977 // -5*n/ln(10)*(2*(x1 - xa)*((x1 - xa)^2 + (y1 - ya)^2)^2 - ((x1 - xa)^2 - (y1 - ya)^2)*4*((x1 - xa)^2 + (y1 - ya)^2)*(x1 - xa))/((x1 - xa)^2 + (y1 - ya)^2)^4*(xi - x1)*(yi - y1)^2
1978
1979 final var fingerprintRssi = point[0];
1980 final var pathLossExponent = point[1];
1981 final var x1 = point[2];
1982 final var y1 = point[3];
1983 final var xa = point[4];
1984 final var ya = point[5];
1985 final var xi = point[6];
1986 final var yi = point[7];
1987
1988 final var diffX1a = x1 - xa;
1989 final var diffY1a = y1 - ya;
1990
1991 final var diffXi1 = xi - x1;
1992 final var diffYi1 = yi - y1;
1993
1994 final var diffX1a2 = diffX1a * diffX1a;
1995 final var diffY1a2 = diffY1a * diffY1a;
1996
1997 final var diffXi12 = diffXi1 * diffXi1;
1998 final var diffYi12 = diffYi1 * diffYi1;
1999
2000 final var diffXi13 = diffXi12 * diffXi1;
2001 final var diffYi13 = diffYi12 * diffYi1;
2002
2003 final var d1a2 = diffX1a2 + diffY1a2;
2004 final var d1a4 = d1a2 * d1a2;
2005 final var d1a8 = d1a4 * d1a4;
2006
2007 final var ln10 = Math.log(10.0);
2008
2009 result[0] = fingerprintRssi
2010 - 10.0 * pathLossExponent * diffX1a / (ln10 * d1a2) * diffXi1
2011 - 10.0 * pathLossExponent * diffY1a / (ln10 * d1a2) * diffYi1
2012 - 5.0 * pathLossExponent * (-diffX1a2 + diffY1a2) / (ln10 * d1a4) * diffXi12
2013 - 5.0 * pathLossExponent * (diffX1a2 - diffY1a2) / (ln10 * d1a4) * diffYi12
2014 + 20.0 * pathLossExponent * diffX1a * diffY1a / (ln10 * d1a4) * diffXi1 * diffYi1
2015 - 10.0 / 6.0 * pathLossExponent / ln10 * (-2.0 * diffX1a * d1a4 - (-diffX1a2 + diffY1a2) * 4.0 * d1a2 * diffX1a) / d1a8 * diffXi13
2016 - 10.0 / 6.0 * pathLossExponent / ln10 * (-2.0 * diffY1a * d1a4 - (diffX1a2 - diffY1a2) * 4.0 * d1a2 * diffY1a) / d1a8 * diffYi13
2017 - 5.0 * pathLossExponent / ln10 * (2.0 * diffY1a * d1a4 - (-diffX1a2 + diffY1a2) * 4.0 * d1a2 * diffY1a) / d1a8 * diffXi12 * diffYi1
2018 - 5.0 * pathLossExponent / ln10 * (2.0 * diffX1a * d1a4 - (diffX1a2 - diffY1a2) * 4.0 * d1a2 * diffX1a) / d1a8 * diffXi1 * diffYi12;
2019 }
2020
2021 @Override
2022 public int getNumberOfVariables() {
2023 return 1;
2024 }
2025 };
2026
2027 final var jacobianEstimator = new JacobianEstimator(evaluator);
2028
2029 return MultivariateNormalDist.propagate(new MultivariateNormalDist.JacobianEvaluator() {
2030 @Override
2031 public void evaluate(final double[] x, final double[] y, final Matrix jacobian) {
2032
2033 try {
2034 evaluator.evaluate(x, y);
2035 jacobianEstimator.jacobian(x, jacobian);
2036 } catch (final EvaluationException ignore) {
2037 //never happens
2038 }
2039 }
2040
2041 @Override
2042 public int getNumberOfVariables() {
2043 return 1;
2044 }
2045 }, mean, covariance);
2046 } catch (final AlgebraException | StatisticsException e) {
2047 throw new IndoorException(e);
2048 }
2049 }
2050
2051 /**
2052 * Propagates provided variances (fingerprint rssi variance, path-loss exponent variance,
2053 * fingerprint position covariance and radio source position covariance) into
2054 * rssi variance by considering the 3D 3rd order Taylor expression of received power.
2055 * Notice that any unknown variance is assumed to be zero.
2056 *
2057 * @param fingerprintRssi closest located fingerprint reading RSSI expressed in dBm's.
2058 * @param pathLossExponent path-loss exponent.
2059 * @param fingerprintPosition position of closest fingerprint.
2060 * @param radioSourcePosition radio source position associated to fingerprint reading.
2061 * @param estimatedPosition position to be estimated. Usually this is equal to the
2062 * initial position used by a non-linear algorithm.
2063 * @param fingerprintRssiVariance variance of fingerprint RSSI or null if unknown.
2064 * @param pathLossExponentVariance variance of path-loss exponent or null if unknown.
2065 * @param fingerprintPositionCovariance covariance of fingerprint position or null if
2066 * unknown.
2067 * @param radioSourcePositionCovariance covariance of radio source position or null
2068 * if unknown.
2069 * @param estimatedPositionCovariance covariance of position to be estimated or null
2070 * if unknown. (This is usually unknown).
2071 * @return a normal distribution containing expected received RSSI value and its variance.
2072 * @throws IndoorException if something fails.
2073 */
2074 public static MultivariateNormalDist propagateVariancesToRssiVarianceThirdOrderNonLinear3D(
2075 final double fingerprintRssi, final double pathLossExponent, final Point3D fingerprintPosition,
2076 final Point3D radioSourcePosition, final Point3D estimatedPosition, final Double fingerprintRssiVariance,
2077 final Double pathLossExponentVariance, final Matrix fingerprintPositionCovariance,
2078 final Matrix radioSourcePositionCovariance, final Matrix estimatedPositionCovariance)
2079 throws IndoorException {
2080
2081 if (fingerprintPosition == null || radioSourcePosition == null || estimatedPosition == null) {
2082 return null;
2083 }
2084
2085 // 1st order Taylor expression of received power in 3D:
2086 // Pr(pi) = Pr(p1)
2087 // - 10*n*(x1 - xa)/(ln(10)*d1a^2)*(xi - x1)
2088 // - 10*n*(y1 - ya)/(ln(10)*d1a^2)*(yi - y1)
2089 // - 10*n*(z1 - za)/(ln(10)*d1a^2)*(zi - z1)
2090 // where d1a^2 = (x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2
2091
2092 final var x1 = fingerprintPosition.getInhomX();
2093 final var y1 = fingerprintPosition.getInhomY();
2094 final var z1 = fingerprintPosition.getInhomZ();
2095
2096 final var xa = radioSourcePosition.getInhomX();
2097 final var ya = radioSourcePosition.getInhomY();
2098 final var za = radioSourcePosition.getInhomZ();
2099
2100 final var xi = estimatedPosition.getInhomX();
2101 final var yi = estimatedPosition.getInhomY();
2102 final var zi = estimatedPosition.getInhomZ();
2103
2104 final var mean = new double[]{
2105 fingerprintRssi, pathLossExponent, x1, y1, z1, xa, ya, za, xi, yi, zi
2106 };
2107 final var covariance = Matrix.diagonal(new double[]{
2108 fingerprintRssiVariance != null ? fingerprintRssiVariance : 0.0,
2109 pathLossExponentVariance != null ? pathLossExponentVariance : 0.0,
2110 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
2111 });
2112
2113 if (fingerprintPositionCovariance != null
2114 && fingerprintPositionCovariance.getRows() == Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH
2115 && fingerprintPositionCovariance.getColumns() == Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH) {
2116
2117 covariance.setSubmatrix(2, 2, 4, 4,
2118 fingerprintPositionCovariance);
2119 }
2120
2121 if (radioSourcePositionCovariance != null
2122 && radioSourcePositionCovariance.getRows() == Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH
2123 && radioSourcePositionCovariance.getColumns() == Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH) {
2124 covariance.setSubmatrix(5, 5, 7, 7,
2125 radioSourcePositionCovariance);
2126 }
2127
2128 if (estimatedPositionCovariance != null
2129 && estimatedPositionCovariance.getRows() == Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH
2130 && estimatedPositionCovariance.getColumns() == Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH) {
2131 covariance.setSubmatrix(8, 8, 10, 10,
2132 estimatedPositionCovariance);
2133 }
2134
2135 try {
2136 //although less precise, we use a jacobian estimator to simplify expressions
2137 final MultiVariateFunctionEvaluatorListener evaluator = new MultiVariateFunctionEvaluatorListener() {
2138 @Override
2139 public void evaluate(final double[] point, final double[] result) {
2140 // Pr(pi = (xi,yi)) = Pr(p1) +
2141 // -10*n*(x1 - xa)/(ln(10)*d1a^2)*(xi - x1) +
2142 // -10*n*(y1 - ya)/(ln(10)*d1a^2)*(yi - y1) +
2143 // -10*n*(z1 - za)/(ln(10)*d1a^2)*(zi - z1) +
2144 // -5*n*((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)/(ln(10)*d1a^4)*(xi - x1)^2 +
2145 // -5*n*((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)/(ln(10)*d1a^4)*(yi - y1)^2 +
2146 // -5*n*((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)/(ln(10)*d1a^4)*(zi - z1)^2 +
2147 // 20*n*(x1 - xa)*(y1 - ya)/(ln(10)*d1a^4)*(xi - x1)*(yi - y1) +
2148 // 20*n*(y1 - ya)*(z1 - za)/(ln(10)*d1a^4)*(yi - y1)*(zi - z1) +
2149 // 20*n*(x1 - xa)*(z1 - za)/(ln(10)*d1a^4)*(xi - x1)*(zi - z1) +
2150 // -10/6*n/ln(10)*(-2*(x1 - xa)*d1a^4 - ((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)*4*d1a^2*(x1 - xa))/d1a^8*(xi - x1)^3 +
2151 // -10/6*n/ln(10)*(-2*(y1 - ya)*d1a^4 - ((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)*4*d1a^2*(y1 - ya))/d1a^8*(yi - y1)^3 +
2152 // -10/6*n/ln(10)*(-2*(z1 - za)*d1a^4 - ((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)*4*d1a^2*(z1 - za))/d1a^8*(zi - z1)^3 +
2153 // -5*n/ln(10)*(2*(y1 - ya)*d1a^4 - ((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)*4*d1a^2*(y1 - ya))/d1a^8*(xi - x1)^2*(yi - y1) +
2154 // -5*n/ln(10)*(2*(z1 - za)*d1a^4 - ((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)*4*d1a^2*(z1 - za))/d1a^8*(xi - x1)^2*(zi - z1) +
2155 // -5*n/ln(10)*(2*(x1 - xa)*d1a^4 - ((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)*4*d1a^2*(x1 - xa))/d1a^8*(xi - x1)*(yi - y1)^2 +
2156 // -5*n/ln(10)*(2*(x1 - xa)*d1a^4 - ((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)*4*d1a^2*(x1 - xa))/d1a^8*(xi - x1)*(zi - z1)^2 +
2157 // -5*n/ln(10)*(2*(z1 - za)*d1a^4 - ((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)*4*d1a^2*(z1 - za))/d1a^8*(yi - y1)^2*(zi - z1) +
2158 // -5*n/ln(10)*(2*(y1 - ya)*d1a^4 - ((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)*4*d1a^2*(y1 - ya))/d1a^8*(yi - y1)*(zi - z1)^2 +
2159 // -80*n/ln(10)*((x1 - xa)*(y1 - ya)*(z1 - za)*d1a^2)/d1a^8*(xi - x1)*(yi - y1)*(zi - z1)
2160
2161 final var fingerprintRssi = point[0];
2162 final var pathLossExponent = point[1];
2163 final var x1 = point[2];
2164 final var y1 = point[3];
2165 final var z1 = point[4];
2166 final var xa = point[5];
2167 final var ya = point[6];
2168 final var za = point[7];
2169 final var xi = point[8];
2170 final var yi = point[9];
2171 final var zi = point[10];
2172
2173 final var diffX1a = x1 - xa;
2174 final var diffY1a = y1 - ya;
2175 final var diffZ1a = z1 - za;
2176
2177 final var diffXi1 = xi - x1;
2178 final var diffYi1 = yi - y1;
2179 final var diffZi1 = zi - z1;
2180
2181 final var diffX1a2 = diffX1a * diffX1a;
2182 final var diffY1a2 = diffY1a * diffY1a;
2183 final var diffZ1a2 = diffZ1a * diffZ1a;
2184
2185 final var diffXi12 = diffXi1 * diffXi1;
2186 final var diffYi12 = diffYi1 * diffYi1;
2187 final var diffZi12 = diffZi1 * diffZi1;
2188
2189 final var diffXi13 = diffXi12 * diffXi1;
2190 final var diffYi13 = diffYi12 * diffYi1;
2191 final var diffZi13 = diffZi12 * diffZi1;
2192
2193 final var d1a2 = diffX1a2 + diffY1a2 + diffZ1a2;
2194 final var d1a4 = d1a2 * d1a2;
2195 final var d1a8 = d1a4 * d1a4;
2196
2197 final var ln10 = Math.log(10.0);
2198
2199 final var value1 = -10.0 * pathLossExponent * diffX1a / (ln10 * d1a2);
2200 final var value2 = -10.0 * pathLossExponent * diffY1a / (ln10 * d1a2);
2201 final var value3 = -10.0 * pathLossExponent * diffZ1a / (ln10 * d1a2);
2202 final var value4 = -5.0 * pathLossExponent * (-diffX1a2 + diffY1a2 + diffZ1a2) / (ln10 * d1a4);
2203 final var value5 = -5.0 * pathLossExponent * (diffX1a2 - diffY1a2 + diffZ1a2) / (ln10 * d1a4);
2204 final var value6 = -5.0 * pathLossExponent * (diffX1a2 + diffY1a2 - diffZ1a2) / (ln10 * d1a4);
2205 final var value7 = 20.0 * pathLossExponent * diffX1a * diffY1a / (ln10 * d1a4);
2206 final var value8 = 20.0 * pathLossExponent * diffY1a * diffZ1a / (ln10 * d1a4);
2207 final var value9 = 20.0 * pathLossExponent * diffX1a * diffZ1a / (ln10 * d1a4);
2208 final var value10 = -10.0 / 6.0 * pathLossExponent / ln10 * (-2.0 * diffX1a * d1a4 - (-diffX1a2 + diffY1a2 + diffZ1a2) * 4.0 * d1a2 * diffX1a) / d1a8;
2209 final var value11 = -10.0 / 6.0 * pathLossExponent / ln10 * (-2.0 * diffY1a * d1a4 - (diffX1a2 - diffY1a2 + diffZ1a2) * 4.0 * d1a2 * diffY1a) / d1a8;
2210 final var value12 = -10.0 / 6.0 * pathLossExponent / ln10 * (-2.0 * diffZ1a * d1a4 - (diffX1a2 + diffY1a2 - diffZ1a2) * 4.0 * d1a2 * diffZ1a) / d1a8;
2211 final var value13 = -5.0 * pathLossExponent / ln10 * (2.0 * diffY1a * d1a4 - (-diffX1a2 + diffY1a2 + diffZ1a2) * 4.0 * d1a2 * diffY1a) / d1a8;
2212 final var value14 = -5.0 * pathLossExponent / ln10 * (2.0 * diffZ1a * d1a4 - (-diffX1a2 + diffY1a2 + diffZ1a2) * 4.0 * d1a2 * diffZ1a) / d1a8;
2213 final var value15 = -5.0 * pathLossExponent / ln10 * (2.0 * diffX1a * d1a4 - (diffX1a2 - diffY1a2 + diffZ1a2) * 4.0 * d1a2 * diffX1a) / d1a8;
2214 final var value16 = -5.0 * pathLossExponent / ln10 * (2.0 * diffX1a * d1a4 - (diffX1a2 + diffY1a2 - diffZ1a2) * 4.0 * d1a2 * diffX1a) / d1a8;
2215 final var value17 = -5.0 * pathLossExponent / ln10 * (2.0 * diffZ1a * d1a4 - (diffX1a2 - diffY1a2 + diffZ1a2) * 4.0 * d1a2 * diffZ1a) / d1a8;
2216 final var value18 = -5.0 * pathLossExponent / ln10 * (2.0 * diffY1a * d1a4 - (diffX1a2 + diffY1a2 - diffZ1a2) * 4.0 * d1a2 * diffY1a) / d1a8;
2217 final var value19 = -80.0 * pathLossExponent / ln10 * (diffX1a * diffY1a * diffZ1a * d1a2) / d1a8;
2218
2219 result[0] = fingerprintRssi
2220 + value1 * diffXi1
2221 + value2 * diffYi1
2222 + value3 * diffZi1
2223 + value4 * diffXi12
2224 + value5 * diffYi12
2225 + value6 * diffZi12
2226 + value7 * diffXi1 * diffYi1
2227 + value8 * diffYi1 * diffZi1
2228 + value9 * diffXi1 * diffZi1
2229 + value10 * diffXi13
2230 + value11 * diffYi13
2231 + value12 * diffZi13
2232 + value13 * diffXi12 * diffYi1
2233 + value14 * diffXi12 * diffZi1
2234 + value15 * diffXi1 * diffYi12
2235 + value16 * diffXi1 * diffZi12
2236 + value17 * diffYi12 * diffZi1
2237 + value18 * diffYi1 * diffZi12
2238 + value19 * diffXi1 * diffYi1 * diffZi1;
2239 }
2240
2241 @Override
2242 public int getNumberOfVariables() {
2243 return 1;
2244 }
2245 };
2246
2247 final var jacobianEstimator = new JacobianEstimator(evaluator);
2248
2249 return MultivariateNormalDist.propagate(new MultivariateNormalDist.JacobianEvaluator() {
2250 @Override
2251 public void evaluate(final double[] x, final double[] y, final Matrix jacobian) {
2252
2253 try {
2254 evaluator.evaluate(x, y);
2255 jacobianEstimator.jacobian(x, jacobian);
2256 } catch (final EvaluationException ignore) {
2257 //never happens
2258 }
2259 }
2260
2261 @Override
2262 public int getNumberOfVariables() {
2263 return 1;
2264 }
2265 }, mean, covariance);
2266 } catch (final AlgebraException | StatisticsException e) {
2267 throw new IndoorException(e);
2268 }
2269 }
2270
2271 /**
2272 * Propagates provided variances (path-loss exponent variance,
2273 * fingerprint position covariance and radio source position covariance) into
2274 * difference of rssi variance by considering the 2D expression.
2275 * Notice that any unknown variance is assumed to be zero.
2276 *
2277 * @param pathLossExponent path-loss exponent.
2278 * @param fingerprintPosition position of closest fingerprint.
2279 * @param radioSourcePosition radio source position associated to fingerprint reading.
2280 * @param estimatedPosition position to be estimated. Usually this is equal to the
2281 * initial position used by a non-linear algorithm.
2282 * @param pathLossExponentVariance variance of path-loss exponent or null if unknown.
2283 * @param fingerprintPositionCovariance covariance of fingerprint position or null if
2284 * unknown.
2285 * @param radioSourcePositionCovariance covariance of radio source position or null
2286 * if unknown.
2287 * @param estimatedPositionCovariance covariance of position to be estimated or null
2288 * if unknown. (This is usually unknown).
2289 * @return a normal distribution containing expected received RSSI value and its variance.
2290 * @throws IndoorException if something fails.
2291 */
2292 public static MultivariateNormalDist propagateVariancesToRssiDifferenceVariance2D(
2293 final double pathLossExponent, final Point2D fingerprintPosition, final Point2D radioSourcePosition,
2294 final Point2D estimatedPosition, final Double pathLossExponentVariance,
2295 final Matrix fingerprintPositionCovariance, final Matrix radioSourcePositionCovariance,
2296 final Matrix estimatedPositionCovariance) throws IndoorException {
2297
2298 if (fingerprintPosition == null || radioSourcePosition == null || estimatedPosition == null) {
2299 return null;
2300 }
2301
2302 // Expression being used is:
2303 // Prdiff1a = Pr(pi) - Pr(p1) = 5*n*log(d1a^2) - 5*n*log(dia^2) =
2304 // = 5*n*log((x1 - xa)^2 + (y1 - ya)^2) - 5*n*log((xi - xa)^2 + (yi - ya)^2)
2305
2306 final var x1 = fingerprintPosition.getInhomX();
2307 final var y1 = fingerprintPosition.getInhomY();
2308
2309 final var xa = radioSourcePosition.getInhomX();
2310 final var ya = radioSourcePosition.getInhomY();
2311
2312 final var xi = estimatedPosition.getInhomX();
2313 final var yi = estimatedPosition.getInhomY();
2314
2315 final var mean = new double[]{
2316 pathLossExponent, x1, y1, xa, ya, xi, yi
2317 };
2318 final var covariance = Matrix.diagonal(new double[]{
2319 pathLossExponentVariance != null ? pathLossExponentVariance : 0.0,
2320 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
2321 });
2322
2323 if (fingerprintPositionCovariance != null
2324 && fingerprintPositionCovariance.getRows() == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH
2325 && fingerprintPositionCovariance.getColumns() == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH) {
2326
2327 covariance.setSubmatrix(1, 1, 2, 2,
2328 fingerprintPositionCovariance);
2329 }
2330
2331 if (radioSourcePositionCovariance != null
2332 && radioSourcePositionCovariance.getRows() == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH
2333 && radioSourcePositionCovariance.getColumns() == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH) {
2334 covariance.setSubmatrix(3, 3, 4, 4,
2335 radioSourcePositionCovariance);
2336 }
2337
2338 if (estimatedPositionCovariance != null
2339 && estimatedPositionCovariance.getRows() == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH
2340 && estimatedPositionCovariance.getColumns() == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH) {
2341 covariance.setSubmatrix(5, 5, 6, 6,
2342 estimatedPositionCovariance);
2343 }
2344
2345 try {
2346 return MultivariateNormalDist.propagate(new MultivariateNormalDist.JacobianEvaluator() {
2347 @Override
2348 public void evaluate(final double[] x, final double[] y, final Matrix jacobian) {
2349
2350 // Expression being used is:
2351 // Prdiff1a = Pr(pi) - Pr(p1) = 5*n*log(d1a^2) - 5*n*log(dia^2) =
2352 // = 5*n*log((x1 - xa)^2 + (y1 - ya)^2) - 5*n*log((xi - xa)^2 + (yi - ya)^2)
2353
2354 final var diffX1a = x1 - xa;
2355 final var diffY1a = y1 - ya;
2356
2357 final var diffXia = xi - xa;
2358 final var diffYia = yi - ya;
2359
2360 final var diffX1a2 = diffX1a * diffX1a;
2361 final var diffY1a2 = diffY1a * diffY1a;
2362
2363 final var diffXia2 = diffXia * diffXia;
2364 final var diffYia2 = diffYia * diffYia;
2365
2366 final var d1a2 = diffX1a2 + diffY1a2;
2367 final var dia2 = diffXia2 + diffYia2;
2368
2369 final var ln10 = Math.log(10.0);
2370
2371 // compute gradient (is a jacobian having 1 row and 7 columns)
2372
2373 // derivative of diff rssi respect to fingerprint path-loss exponent "n"
2374
2375 final var derivativePathLossExponent = 5.0 * (Math.log10(d1a2) - Math.log10(dia2));
2376
2377 // derivative of diff rssi respect to x1
2378 // diff(Prdiff1a)/diff(x1) = 5*n/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2))*2*(x1 - xa) =
2379 // = 10*n*(x1 - xa)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2))
2380
2381 final var tmp1a = 10.0 * pathLossExponent / (ln10 * d1a2);
2382 final var derivativeX1 = tmp1a * diffX1a;
2383
2384 // derivative of diff rssi respect to y1
2385 // diff(Prdiff1a)/diff(y1) = 5*n/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2))*2*(y1 - ya) =
2386 // = 10*n*(y1 - ya)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2))
2387
2388 final var derivativeY1 = tmp1a * diffY1a;
2389
2390 // derivative of rssi respect to xi
2391 // diff(Prdiff1a)/diff(xi) = -5*n/(ln(10)*((xi - xa)^2 + (yi - ya)^2))*2*(xi - xa)
2392 // = -10*n*(xi - xa)/(ln(10)*((xi - xa)^2 + (yi - ya)^2))
2393
2394 final var tmpia = 10.0 * pathLossExponent / (ln10 * dia2);
2395 final var derivativeXi = -tmpia * diffXia;
2396
2397 // derivative of rssi respect to yi
2398 // diff(Prdiff1a)/diff(yi) = -5*n/(ln(10)*((xi - xa)^2 + (yi - ya)^2))*2*(yi - ya)
2399 // = -10*n*(yi - ya)/(ln(10)*((xi - xa)^2 + (yi - ya)^2))
2400
2401 final var derivativeYi = -tmpia * diffYia;
2402
2403 // derivative of rssi respect to xa
2404 // diff(Prdiff1a)/diff(xa) = 5*n/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2))*-2*(x1 - xa) -5*n/(ln(10)*((xi - xa)^2 + (yi - ya)^2))*-2(xi - xa) =
2405 // = -10*n*(x1 - xa)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2)) + 10*n*(xi - xa)/(ln(10)*((xi - xa)^2 + (yi - ya)^2))
2406
2407 final var derivativeXa = -derivativeX1 - derivativeXi;
2408
2409 // derivative of rssi respect to ya
2410 // diff(Prdiff1a)/diff(ya) = 5*n/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2))*-2*(y1 - ya) -5*n/(ln(10)*((xi - xa)^2 + (yi - ya)^2))*-2(yi - ya) =
2411 // = -10*n*(y1 - ya)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2)) + 10*n*(yi - ya)/(ln(10)*((xi - xa)^2 + (yi - ya)^2))
2412
2413 final var derivativeYa = -derivativeY1 - derivativeYi;
2414
2415 // Prdiff1a = Pr(pi) - Pr(p1) = 5*n*log(d1a^2) - 5*n*log(dia^2) =
2416 // = 5*n*log((x1 - xa)^2 + (y1 - ya)^2) - 5*n*log((xi - xa)^2 + (yi - ya)^2)
2417
2418 // set derivatives pathLossExponent, x1, y1, xa, ya, xi, yi
2419 jacobian.setElementAtIndex(0, derivativePathLossExponent);
2420 jacobian.setElementAtIndex(1, derivativeX1);
2421 jacobian.setElementAtIndex(2, derivativeY1);
2422 jacobian.setElementAtIndex(3, derivativeXa);
2423 jacobian.setElementAtIndex(4, derivativeYa);
2424 jacobian.setElementAtIndex(5, derivativeXi);
2425 jacobian.setElementAtIndex(6, derivativeYi);
2426
2427
2428 y[0] = derivativePathLossExponent * pathLossExponent;
2429 }
2430
2431 @Override
2432 public int getNumberOfVariables() {
2433 return 1;
2434 }
2435 }, mean, covariance);
2436 } catch (final AlgebraException | StatisticsException e) {
2437 throw new IndoorException(e);
2438 }
2439 }
2440
2441 /**
2442 * Propagates provided variances (path-loss exponent variance,
2443 * fingerprint position covariance and radio source position covariance) into
2444 * difference of rssi variance by considering the 3D expression.
2445 * Notice that any unknown variance is assumed to be zero.
2446 *
2447 * @param pathLossExponent path-loss exponent.
2448 * @param fingerprintPosition position of closest fingerprint.
2449 * @param radioSourcePosition radio source position associated to fingerprint reading.
2450 * @param estimatedPosition position to be estimated. Usually this is equal to the
2451 * initial position used by a non-linear algorithm.
2452 * @param pathLossExponentVariance variance of path-loss exponent or null if unknown.
2453 * @param fingerprintPositionCovariance covariance of fingerprint position or null if
2454 * unknown.
2455 * @param radioSourcePositionCovariance covariance of radio source position or null
2456 * if unknown.
2457 * @param estimatedPositionCovariance covariance of position to be estimated or null
2458 * if unknown. (This is usually unknown).
2459 * @return a normal distribution containing expected received RSSI value and its variance.
2460 * @throws IndoorException if something fails.
2461 */
2462 public static MultivariateNormalDist propagateVariancesToRssiDifferenceVariance3D(
2463 final double pathLossExponent, final Point3D fingerprintPosition, final Point3D radioSourcePosition,
2464 final Point3D estimatedPosition, final Double pathLossExponentVariance,
2465 final Matrix fingerprintPositionCovariance, final Matrix radioSourcePositionCovariance,
2466 final Matrix estimatedPositionCovariance) throws IndoorException {
2467
2468 if (fingerprintPosition == null || radioSourcePosition == null || estimatedPosition == null) {
2469 return null;
2470 }
2471
2472 // Expression being used is:
2473 // Prdiff1a = Pr(pi) - Pr(p1) = 5*n*log(d1a^2) - 5*n*log(dia^2) =
2474 // = 5*n*log((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2) - 5*n*log((xi - xa)^2 + (yi - ya)^2 + (zi - za)^2)
2475
2476 final var x1 = fingerprintPosition.getInhomX();
2477 final var y1 = fingerprintPosition.getInhomY();
2478 final var z1 = fingerprintPosition.getInhomZ();
2479
2480 final var xa = radioSourcePosition.getInhomX();
2481 final var ya = radioSourcePosition.getInhomY();
2482 final var za = radioSourcePosition.getInhomZ();
2483
2484 final var xi = estimatedPosition.getInhomX();
2485 final var yi = estimatedPosition.getInhomY();
2486 final var zi = estimatedPosition.getInhomZ();
2487
2488 final var mean = new double[]{
2489 pathLossExponent, x1, y1, z1, xa, ya, za, xi, yi, zi
2490 };
2491 final var covariance = Matrix.diagonal(new double[]{
2492 pathLossExponentVariance != null ? pathLossExponentVariance : 0.0,
2493 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
2494 });
2495
2496 if (fingerprintPositionCovariance != null
2497 && fingerprintPositionCovariance.getRows() == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH
2498 && fingerprintPositionCovariance.getColumns() == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH) {
2499
2500 covariance.setSubmatrix(1, 1, 3, 3,
2501 fingerprintPositionCovariance);
2502 }
2503
2504 if (radioSourcePositionCovariance != null
2505 && radioSourcePositionCovariance.getRows() == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH
2506 && radioSourcePositionCovariance.getColumns() == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH) {
2507 covariance.setSubmatrix(4, 4, 6, 6,
2508 radioSourcePositionCovariance);
2509 }
2510
2511 if (estimatedPositionCovariance != null
2512 && estimatedPositionCovariance.getRows() == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH
2513 && estimatedPositionCovariance.getColumns() == Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH) {
2514 covariance.setSubmatrix(7, 7, 9, 9,
2515 estimatedPositionCovariance);
2516 }
2517
2518 try {
2519 return MultivariateNormalDist.propagate(new MultivariateNormalDist.JacobianEvaluator() {
2520 @Override
2521 public void evaluate(final double[] x, final double[] y, final Matrix jacobian) {
2522
2523 // Expression being used is:
2524 // Prdiff1a = Pr(pi) - Pr(p1) = 5*n*log(d1a^2) - 5*n*log(dia^2) =
2525 // = 5*n*log((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2) - 5*n*log((xi - xa)^2 + (yi - ya)^2 + (zi - za)^2)
2526
2527 final var diffX1a = x1 - xa;
2528 final var diffY1a = y1 - ya;
2529 final var diffZ1a = z1 - za;
2530
2531 final var diffXia = xi - xa;
2532 final var diffYia = yi - ya;
2533 final var diffZia = zi - za;
2534
2535 final var diffX1a2 = diffX1a * diffX1a;
2536 final var diffY1a2 = diffY1a * diffY1a;
2537 final var diffZ1a2 = diffZ1a * diffZ1a;
2538
2539 final var diffXia2 = diffXia * diffXia;
2540 final var diffYia2 = diffYia * diffYia;
2541 final var diffZia2 = diffZia * diffZia;
2542
2543 final var d1a2 = diffX1a2 + diffY1a2 + diffZ1a2;
2544 final var dia2 = diffXia2 + diffYia2 + diffZia2;
2545
2546 final var ln10 = Math.log(10.0);
2547
2548 // compute gradient (is a jacobian having 1 row and 10 columns)
2549
2550 // derivative of diff rssi respect to fingerprint path-loss exponent "n"
2551
2552 final var derivativePathLossExponent = 5.0 * (Math.log10(d1a2) - Math.log10(dia2));
2553
2554 // derivative of diff rssi respect to x1
2555 // diff(Prdiff1a)/diff(x1) = 5*n/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2))*2*(x1 - xa) =
2556 // = 10*n*(x1 - xa)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2))
2557
2558 final var tmp1a = 10.0 * pathLossExponent / (ln10 * d1a2);
2559 final var derivativeX1 = tmp1a * diffX1a;
2560
2561 // derivative of diff rssi respect to y1
2562 // diff(Prdiff1a)/diff(y1) = 5*n/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2) + (z1 - za)^2)*2*(y1 - ya) =
2563 // = 10*n*(y1 - ya)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2))
2564
2565 final var derivativeY1 = tmp1a * diffY1a;
2566
2567 // derivative of diff rssi respect to z1
2568 // diff(Prdiff1a)/diff(z1) = 5*n/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2) + (z1 - za)^2)*2*(z1 - za) =
2569 // = 10*n*(z1 - za)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2))
2570
2571 final var derivativeZ1 = tmp1a * diffZ1a;
2572
2573 // derivative of rssi respect to xi
2574 // diff(Prdiff1a)/diff(xi) = -5*n/(ln(10)*((xi - xa)^2 + (yi - ya)^2 + (zi - za)^2))*2*(xi - xa)
2575 // = -10*n*(xi - xa)/(ln(10)*((xi - xa)^2 + (yi - ya)^2 + (zi - za)^2))
2576
2577 final var tmpia = 10.0 * pathLossExponent / (ln10 * dia2);
2578 final var derivativeXi = -tmpia * diffXia;
2579
2580 // derivative of rssi respect to yi
2581 // diff(Prdiff1a)/diff(yi) = -5*n/(ln(10)*((xi - xa)^2 + (yi - ya)^2 + (zi - za)^2))*2*(yi - ya)
2582 // = -10*n*(yi - ya)/(ln(10)*((xi - xa)^2 + (yi - ya)^2 + (zi - za)^2))
2583
2584 final var derivativeYi = -tmpia * diffYia;
2585
2586 // derivative of rssi respect to zi
2587 // diff(Prdiff1a)/diff(zi) = -5*n/(ln(10)*((xi - xa)^2 + (yi - ya)^2 + (zi - za)^2))*2*(zi - za)
2588 // = -10*n*(zi - za)/(ln(10)*((xi - xa)^2 + (yi - ya)^2 + (zi - za)^2))
2589
2590 final var derivativeZi = -tmpia * diffZia;
2591
2592 // Prdiff1a = Pr(pi) - Pr(p1) = 5*n*log(d1a^2) - 5*n*log(dia^2) =
2593 // = 5*n*log((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2) - 5*n*log((xi - xa)^2 + (yi - ya)^2 + (zi - za)^2)
2594
2595 // derivative of rssi respect to xa
2596 // diff(Prdiff1a)/diff(xa) = 5*n/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2))*-2*(x1 - xa) -5*n/(ln(10)*((xi - xa)^2 + (yi - ya)^2 + (zi - za)^2))*-2(xi - xa) =
2597 // = -10*n*(x1 - xa)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)) + 10*n*(xi - xa)/(ln(10)*((xi - xa)^2 + (yi - ya)^2 + (zi - za)^2))
2598
2599 final var derivativeXa = -derivativeX1 - derivativeXi;
2600
2601 // derivative of rssi respect to ya
2602 // diff(Prdiff1a)/diff(ya) = 5*n/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2))*-2*(y1 - ya) -5*n/(ln(10)*((xi - xa)^2 + (yi - ya)^2 + (zi - za)^2))*-2(yi - ya) =
2603 // = -10*n*(y1 - ya)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)) + 10*n*(yi - ya)/(ln(10)*((xi - xa)^2 + (yi - ya)^2 + (zi - za)^2))
2604
2605 final var derivativeYa = -derivativeY1 - derivativeYi;
2606
2607 // derivative of rssi respect to za
2608 // diff(Prdiff1a)/diff(za) = 5*n/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2))*-2*(z1 - za) -5*n/(ln(10)*((xi - xa)^2 + (yi - ya)^2 + (zi - za)^2))*-2(zi - za) =
2609 // = -10*n*(z1 - za)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)) + 10*n*(zi - za)/(ln(10)*((xi - xa)^2 + (yi - ya)^2 + (zi - za)^2))
2610
2611 final var derivativeZa = -derivativeZ1 - derivativeZi;
2612
2613 // set derivatives pathLossExponent, x1, y1, z1, xa, ya, za, xi, yi, zi
2614 jacobian.setElementAtIndex(0, derivativePathLossExponent);
2615 jacobian.setElementAtIndex(1, derivativeX1);
2616 jacobian.setElementAtIndex(2, derivativeY1);
2617 jacobian.setElementAtIndex(3, derivativeZ1);
2618 jacobian.setElementAtIndex(4, derivativeXa);
2619 jacobian.setElementAtIndex(5, derivativeYa);
2620 jacobian.setElementAtIndex(6, derivativeZa);
2621 jacobian.setElementAtIndex(7, derivativeXi);
2622 jacobian.setElementAtIndex(8, derivativeYi);
2623 jacobian.setElementAtIndex(9, derivativeZi);
2624
2625
2626 y[0] = derivativePathLossExponent * pathLossExponent;
2627 }
2628
2629 @Override
2630 public int getNumberOfVariables() {
2631 return 1;
2632 }
2633 }, mean, covariance);
2634 } catch (final AlgebraException | StatisticsException e) {
2635 throw new IndoorException(e);
2636 }
2637 }
2638 }