1 /*
2 * Copyright (C) 2018 Alberto Irurueta Carro (alberto@irurueta.com)
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16 package com.irurueta.navigation.indoor.fingerprint;
17
18 import com.irurueta.algebra.Matrix;
19 import com.irurueta.geometry.Point3D;
20 import com.irurueta.navigation.indoor.IndoorException;
21 import com.irurueta.navigation.indoor.RadioSource;
22 import com.irurueta.navigation.indoor.RadioSourceLocated;
23 import com.irurueta.navigation.indoor.RssiFingerprint;
24 import com.irurueta.navigation.indoor.RssiFingerprintLocated;
25 import com.irurueta.navigation.indoor.RssiReading;
26 import com.irurueta.navigation.indoor.Utils;
27
28 import java.util.List;
29
30 /**
31 * 2D position estimator based on located fingerprints containing only RSSI readings and
32 * having as well prior knowledge of the location of radio sources associated to those
33 * readings.
34 * This implementation uses a third-order Taylor approximation over provided located
35 * fingerprints to determine an approximate position for a non-located fingerprint using
36 * a non-linear solving algorithm.
37 * An initial position can be provided as a starting point to solve the position,
38 * otherwise the average point of selected nearest fingerprints is used as a starting
39 * point.
40 */
41 public class ThirdOrderNonLinearFingerprintPositionEstimator3D extends NonLinearFingerprintPositionEstimator3D {
42
43 /**
44 * Constructor.
45 */
46 public ThirdOrderNonLinearFingerprintPositionEstimator3D() {
47 }
48
49 /**
50 * Constructor.
51 *
52 * @param listener listener in charge of handling events.
53 */
54 public ThirdOrderNonLinearFingerprintPositionEstimator3D(
55 final FingerprintPositionEstimatorListener<Point3D> listener) {
56 super(listener);
57 }
58
59 /**
60 * Constructor.
61 *
62 * @param locatedFingerprints located fingerprints containing RSSI readings.
63 * @param fingerprint fingerprint containing readings at an unknown location
64 * for provided located fingerprints.
65 * @param sources located radio sources.
66 * @throws IllegalArgumentException if provided non located fingerprint is null,
67 * located fingerprints value is null or there are not enough fingerprints or
68 * readings within provided fingerprints (for 3D position estimation 3 located
69 * total readings are required among all fingerprints).
70 */
71 public ThirdOrderNonLinearFingerprintPositionEstimator3D(
72 final List<? extends RssiFingerprintLocated<? extends RadioSource,
73 ? extends RssiReading<? extends RadioSource>, Point3D>> locatedFingerprints,
74 final RssiFingerprint<? extends RadioSource,
75 ? extends RssiReading<? extends RadioSource>> fingerprint,
76 final List<? extends RadioSourceLocated<Point3D>> sources) {
77 super(locatedFingerprints, fingerprint, sources);
78 }
79
80 /**
81 * Constructor.
82 *
83 * @param locatedFingerprints located fingerprints containing RSSI readings.
84 * @param fingerprint fingerprint containing readings at an unknown location
85 * for provided located fingerprints.
86 * @param sources located radio sources.
87 * @param listener listener in charge of handling events.
88 * @throws IllegalArgumentException if provided non located fingerprint is null,
89 * located fingerprints value is null or there are not enough fingerprints or
90 * readings within provided fingerprints (for 3D position estimation 3 located
91 * total readings are required among all fingerprints).
92 */
93 public ThirdOrderNonLinearFingerprintPositionEstimator3D(
94 final List<? extends RssiFingerprintLocated<? extends RadioSource,
95 ? extends RssiReading<? extends RadioSource>, Point3D>> locatedFingerprints,
96 final RssiFingerprint<? extends RadioSource,
97 ? extends RssiReading<? extends RadioSource>> fingerprint,
98 final List<? extends RadioSourceLocated<Point3D>> sources,
99 final FingerprintPositionEstimatorListener<Point3D> listener) {
100 super(locatedFingerprints, fingerprint, sources, listener);
101 }
102
103 /**
104 * Constructor.
105 *
106 * @param locatedFingerprints located fingerprints containing RSSI readings.
107 * @param fingerprint fingerprint containing readings at an unknown location
108 * for provided located fingerprints.
109 * @param sources located radio sources.
110 * @param initialPosition initial position to start the solving algorithm or null.
111 * @throws IllegalArgumentException if provided non located fingerprint is null,
112 * located fingerprints value is null or there are not enough fingerprints or
113 * readings within provided fingerprints (for 3D position estimation 3 located
114 * * total readings are required among all fingerprints).
115 */
116 public ThirdOrderNonLinearFingerprintPositionEstimator3D(
117 final List<? extends RssiFingerprintLocated<? extends RadioSource,
118 ? extends RssiReading<? extends RadioSource>, Point3D>> locatedFingerprints,
119 final RssiFingerprint<? extends RadioSource,
120 ? extends RssiReading<? extends RadioSource>> fingerprint,
121 final List<? extends RadioSourceLocated<Point3D>> sources, Point3D initialPosition) {
122 super(locatedFingerprints, fingerprint, sources, initialPosition);
123 }
124
125 /**
126 * Constructor.
127 *
128 * @param locatedFingerprints located fingerprints containing RSSI readings.
129 * @param fingerprint fingerprint containing readings at an unknown location
130 * for provided located fingerprints.
131 * @param sources located radio sources.
132 * @param initialPosition initial position to start the solving algorithm or null.
133 * @param listener listener in charge of handling events.
134 * @throws IllegalArgumentException if provided non located fingerprint is null,
135 * located fingerprints value is null or there are not enough fingerprints or
136 * readings within provided fingerprints (for 3D position estimation 3 located
137 * * * total readings are required among all fingerprints).
138 */
139 public ThirdOrderNonLinearFingerprintPositionEstimator3D(
140 final List<? extends RssiFingerprintLocated<? extends RadioSource,
141 ? extends RssiReading<? extends RadioSource>, Point3D>> locatedFingerprints,
142 final RssiFingerprint<? extends RadioSource,
143 ? extends RssiReading<? extends RadioSource>> fingerprint,
144 final List<? extends RadioSourceLocated<Point3D>> sources, Point3D initialPosition,
145 final FingerprintPositionEstimatorListener<Point3D> listener) {
146 super(locatedFingerprints, fingerprint, sources, initialPosition, listener);
147 }
148
149 /**
150 * Gets type of position estimator.
151 *
152 * @return type of position estimator.
153 */
154 @Override
155 public NonLinearFingerprintPositionEstimatorType getType() {
156 return NonLinearFingerprintPositionEstimatorType.THIRD_ORDER;
157 }
158
159 /**
160 * Evaluates a non-linear multi dimension function at provided point using
161 * provided parameters and returns its evaluation and derivatives of the
162 * function respect the function parameters.
163 *
164 * @param i number of sample being evaluated.
165 * @param point point where function will be evaluated.
166 * @param params initial parameters estimation to be tried. These will
167 * change as the Levenberg-Marquardt algorithm iterates to the best solution.
168 * These are used as input parameters along with point to evaluate function.
169 * @param derivatives partial derivatives of the function respect to each
170 * provided parameter.
171 * @return function evaluation at provided point.
172 */
173 @Override
174 @SuppressWarnings("Duplicates")
175 protected double evaluate(
176 final int i, final double[] point, final double[] params, final double[] derivatives) {
177 // Demonstration in 3D:
178 // --------------------
179 // Taylor series expansion can be expressed as:
180 // f(x) = f(a) + 1/1!*f'(a)*(x - a) + 1/2!*f''(a)*(x - a)^2 + 1/3!*f'''(a)*(x - a)^3 ...
181
182 // where f'(x) is the derivative of f respect x, which can also be expressed as:
183 // f'(x) = diff(f(x))/diff(x)
184
185 // and f'(a) is the derivative of f respect x evaluated at "a", which can be expressed
186 // as f'(a) = diff(f(a))/diff(x)
187
188 // consequently f''(a) is the second derivative respect x evaluated at "a", which can
189 // be expressed as:
190 // f''(x) = diff(f(x))/diff(x^2)
191
192 // and:
193 // f''(a) = diff(f(a))/diff(x^2)
194
195 // and finally f'''(a) is the third derivative respect x evaluated at "a", which can
196 // be expressed as:
197 // f'''(x) = diff(f(x))/diff(x^3)
198
199 // and:
200 // f'''(a) = diff(f(a))/diff(x^3)
201
202 // Received power expressed in dBm is:
203 // k = (c/(4*pi*f))
204 // Pr = Pte*k^n / d^n
205
206 // where c is the speed of light, pi is 3.14159..., f is the frequency of the radio source,
207 // Pte is the equivalent transmitted power by the radio source, n is the path-loss exponent
208 // (typically 2.0), and d is the distance from a point to the location of the radio source.
209
210 // Hence:
211 // Pr(dBm) = 10*log(Pte*k^n/d^n) = 10*n*log(k) + 10*log(Pte) - 10*n*log(d) =
212 // 10*n*log(k) + 10*log(Pte) - 5*n*log(d^2)
213
214 // The former 2 terms are constant, and only the last term depends on distance
215
216 // Hence, assuming the constant K = 10*n*log(k) + Pte(dBm), where Pte(dBm) = 10*log(Pte),
217 // assuming that transmitted power by the radio source Pte is known (so that K is also known),
218 // and assuming that the location of the radio source is known, and it is located at pa = (xa, ya)
219 // so that d^2 = (x - xa)^2 + (y - ya)^2 then the received power at an unknown point pi = (xi, yi) is:
220
221 // Pr(pi) = Pr(xi,yi) = K - 5*n*log(d^2) = K - 5*n*log((xi - xa)^2 + (yi - ya)^2)
222
223 // Suppose that received power at point p1=(x1,y1) is known on a located fingerprint
224 // containing readings Pr(p1).
225
226 // Then, for an unknown point pi=(xi,yi) close to fingerprint 1 located at p1 where we
227 // have measured received power Pr(pi), we can get the following third-order Taylor
228 // approximation:
229
230 // Pr(pi = (xi,yi)) = Pr(p1) +
231 // diff(Pr(p1))/diff(x)*(xi - x1) +
232 // diff(Pr(p1))/diff(y)*(yi - y1) +
233 // diff(Pr(p1))/diff(z)*(zi - z1) +
234 // 1/2*(diff(Pr(p1))/diff(x^2)*(xi - x1)^2 +
235 // diff(Pr(p1))/diff(y^2)*(yi - y1)^2 +
236 // diff(Pr(p1))/diff(z^2)*(zi - z1)^2 +
237 // 2*diff(Pr(p1))/diff(x*y)*(xi - x1)*(yi - y1)) +
238 // 2*diff(Pr(p1))/diff(y*z)*(yi - y1)*(zi - z1) +
239 // 2*diff(Pr(p1))/diff(x*z)*(xi - x1)*(zi - z1)
240 // 1/6*(diff(Pr(p1))/diff(x^3)*(xi - x1)^3 +
241 // diff(Pr(p1))/diff(y^3)*(yi - y1)^3 +
242 // diff(Pr(p1))/diff(z^3)*(zi - z1)^3 +
243 // 3*diff(Pr(p1))/diff(x^2*y)*(xi - x1)^2*(yi - y1) +
244 // 3*diff(Pr(p1))/diff(x^2*z)*(xi - x1)^2*(zi - z1) +
245 // 3*diff(Pr(p1))/diff(x*y^2)*(xi - x1)*(yi - y1)^2 +
246 // 3*diff(Pr(p1))/diff(x*z^2)*(xi - x1)*(zi - z1)^2 +
247 // 3*diff(Pr(p1))/diff(y^2*z)*(yi - y1)^2*(zi - z1) +
248 // 3*diff(Pr(p1))/diff(y*z^2)*(yi - y1)*(zi - z1)^2 +
249 // 6*diff(Pr(p1))/diff(x*y*z)*(xi - x1)*(yi - y1)*(zi - z1)
250
251 // where the first order derivatives of Pr(p = (x,y)) are:
252 // diff(Pr(x,y,z))/diff(x) = -5*n/(ln(10)*((x - xa)^2 + (y - ya)^2 + (z - za)^2)*2*(x - xa)
253 // diff(Pr(x,y,z))/diff(x) = -10*n*(x - xa)/(ln(10)*((x - xa)^2 + (y - ya)^2 + (z - za)^2))
254
255 // diff(Pr(x,y,z))/diff(y) = -5*n/(ln(10)*((x - xa)^2 + (y - ya)^2 + (z - za)^2)*2*(y - ya)
256 // diff(Pr(x,y,z))/diff(y) = -10*n*(y - ya)/(ln(10)*((x - xa)^2 + (y - ya)^2 + (z - za)^2))
257
258 // diff(Pr(x,y,z))/diff(z) = -5*n/(ln(10)*((x - xa)^2 + (y - ya)^2 + (z - za)^2)*2*(z - za)
259 // diff(Pr(x,y,z))/diff(z) = -10*n*(z - za)/(ln(10)*((x - xa)^2 + (y - ya)^2 + (z - za)^2))
260
261 // If we evaluate first order derivatives at p1 = (x1,y1), we get:
262 // diff(Pr(p1))/diff(x) = -10*n*(x1 - xa)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2))
263 // diff(Pr(p1))/diff(y) = -10*n*(y1 - ya)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2))
264 // diff(Pr(p1))/diff(z) = -10*n*(z1 - za)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2))
265
266 // where square distance from fingerprint 1 to radio source a can be expressed as:
267 // d1a^2 = (x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2
268
269 // where both the fingerprint and radio source positions are known, and hence d1a is known.
270
271 // Then first order derivatives can be expressed as:
272 // diff(Pr(p1))/diff(x) = -10*n*(x1 - xa)/(ln(10)*d1a^2)
273 // diff(Pr(p1))/diff(y) = -10*n*(y1 - ya)/(ln(10)*d1a^2)
274 // diff(Pr(p1))/diff(z) = -10*n*(z1 - za)/(ln(10)*d1a^2)
275
276 // To obtain second order derivatives we take into account that:
277 // (f(x)/g(x))' = (f'(x)*g(x) - f(x)*g'(x))/g(x)^2
278
279 // hence, second order derivatives of Pr(p = (x,y,z)) are:
280 // diff(Pr(x,y,z))/diff(x^2) = -10*n/ln(10)*(1*((x - xa)^2 + (y - ya)^2 + (z - za)^2) - (x - xa)*2*(x - xa))/((x - xa)^2 + (y - ya)^2 + (z - za)^2)^2
281 // diff(Pr(x,y,z))/diff(x^2) = -10*n*((y - ya)^2 + (z - za)^2 - (x - xa)^2)/(ln(10)*((x - xa)^2 + (y - ya)^2 + (z - za)^2)^2)
282
283 // diff(Pr(x,y,z))/diff(y^2) = -10*n/ln(10)*(1*((x - xa)^2 + (y - ya)^2 + (z - za)^2) - (y - ya)*2*(y - ya))/((x - xa)^2 + (y - ya)^2 + (z - za)^2)^2
284 // diff(Pr(x,y,z))/diff(y^2) = -10*n*((x - xa)^2 - (y - ya)^2 + (z - za)^2)/(ln(10)*((x - xa)^2 + (y - ya)^2 + (z - za)^2)^2)
285
286 // diff(Pr(x,y,z))/diff(z^2) = -10*n/ln(10)*(1*((x - xa)^2 + (y - ya)^2 + (z - za)^2) - (z - za)*2*(z - za))/((x - xa)^2 + (y - ya)^2 + (z - za)^2)^2
287 // diff(Pr(x,y,z))/diff(z^2) = -10*n*((x - xa)^2 + (y - ya)^2 - (z - za)^2)/(ln(10)*((x - xa)^2 + (y - ya)^2 + (z - za)^2)^2)
288
289 // diff(Pr(x,y,z))/diff(x*y) = -10*n/ln(10)*(0*((x - xa)^2 + (y - ya)^2 + (z - za)^2) - (x - xa)*2*(y - ya))/((x - xa)^2 + (y - ya)^2 + (z - za)^2)^2
290 // diff(Pr(x,y,z))/diff(x*y) = 20*n*(x - xa)*(y - ya)/(ln(10)*((x - xa)^2 + (y - ya)^2 + (z - za)^2)^2)
291
292 // diff(Pr(x,y,z))/diff(x*z) = -10*n/ln(10)*(0*((x - xa)^2 + (y - ya)^2 + (z - za)^2) - (x - xa)*2*(z - za))/((x - xa)^2 + (y - ya)^2 + (z - za)^2)^2
293 // diff(Pr(x,y,z))/diff(x*z) = 20*n*(x - xa)*(z - za)/(ln(10)*((x - xa)^2 + (y - ya)^2 + (z - za)^2)^2)
294
295 // diff(Pr(x,y,z))/diff(y*z) = -10*n/ln(10)*(0*((x - xa)^2 + (y - ya)^2 + (z - za)^2) - (y - ya)*2*(z - za))/((x - xa)^2 + (y - ya)^2 + (z - za)^2)^2
296 // diff(Pr(x,y,z))/diff(y*z) = 20*n*(y - ya)*(z - za)/(ln(10)*((x - xa)^2 + (y - ya)^2 + (z - za)^2)^2)
297
298 // If we evaluate second order derivatives at p1 = (x1,y1,z1), we get:
299 // diff(Pr(p1))/diff(x^2) = -10*n*((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)
300 // diff(Pr(p1))/diff(y^2) = -10*n*((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)
301 // diff(Pr(p1))/diff(z^2) = -10*n*((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)
302 // diff(Pr(p1))/diff(x*y) = 20*n*(x1 - xa)*(y1 - ya)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)
303 // diff(Pr(p1))/diff(x*z) = 20*n*(x1 - xa)*(z1 - za)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)
304 // diff(Pr(p1))/diff(y*z) = 20*n*(y1 - ya)*(z1 - za)/(ln(10)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2)
305
306 // and expressing the second order derivatives in terms of distance between
307 // fingerprint 1 and radio source a d1a, we get:
308 // diff(Pr(p1))/diff(x^2) = -10*n*((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)/(ln(10)*d1a^4)
309 // diff(Pr(p1))/diff(y^2) = -10*n*((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)/(ln(10)*d1a^4)
310 // diff(Pr(p1))/diff(z^2) = -10*n*((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)/(ln(10)*d1a^4)
311 // diff(Pr(p1))/diff(x*y) = 20*n*(x1 - xa)*(y1 - ya)/(ln(10)*d1a^4)
312 // diff(Pr(p1))/diff(x*z) = 20*n*(x1 - xa)*(z1 - za)/(ln(10)*d1a^4)
313 // diff(Pr(p1))/diff(y*z) = 20*n*(y1 - ya)*(z1 - za)/(ln(10)*d1a^4)
314
315 // Finally, third order derivatives of Pr(p = (x,y,z)) are:
316 // diff(Pr(x,y,z))/diff(x^3) = -10*n/ln(10)*(-2*(x - xa)*((x - xa)^2 + (y - ya)^2 + (z - za)^2)^2 - ((y - ya)^2 + (z - za)^2 - (x - xa)^2)*2*((x - xa)^2 + (y - ya)^2 + (z - za)^2)*2*(x - xa))/((x - xa)^2 + (y - ya)^2 + (z - za)^2)^4
317 // diff(Pr(x,y,z))/diff(y^2) = -10*n/ln(10)*(-2*(y - ya)*((x - xa)^2 + (y - ya)^2 + (z - za)^2)^2 - ((x - xa)^2 - (y - ya)^2 + (z - za)^2)*2*((x - xa)^2 + (y - ya)^2 + (z - za)^2)*2*(y - ya))/((x - xa)^2 + (y - ya)^2 + (z - za)^2)^4
318 // diff(Pr(x,y,z))/diff(z^3) = -10*n/ln(10)*(-2*(z - za)*((x - xa)^2 + (y - ya)^2 + (z - za)^2)^2 - ((x - xa)^2 + (y - ya)^2 - (z - za)^2)*2*((x - xa)^2 + (y - ya)^2 + (z - za)^2)*2*(z - za))/((x - xa)^2 + (y - ya)^2 + (z - za)^2)^4
319 // diff(Pr(x,y,z))/diff(x^2*y) = -10*n/ln(10)*(2*(y - ya)*((x - xa)^2 + (y - ya)^2 + (z - za)^2)^2 - ((y - ya)^2 + (z - za)^2 - (x - xa)^2)*2*((x - xa)^2 + (y - ya)^2 + (z - za)^2)*2*(y - ya))/((x - xa)^2 + (y - ya)^2 + (z - za)^2)^4
320 // diff(Pr(x,y,z))/diff(x^2*z) = -10*n/ln(10)*(2*(z - za)*((x - xa)^2 + (y - ya)^2 + (z - za)^2)^2 - ((y - ya)^2 + (z - za)^2 - (x - xa)^2)*2*((x - xa)^2 + (y - ya)^2 + (z - za)^2)*2*(z - za))/((x - xa)^2 + (y - ya)^2 + (z - za)^2)^4
321 // diff(Pr(x,y,z))/diff(x*y^2) = -10*n/ln(10)*(2*(x - xa)*((x - xa)^2 + (y - ya)^2 + (z - za)^2)^2 - ((x - xa)^2 - (y - ya)^2 + (z - za)^2)*2*((x - xa)^2 + (y - ya)^2 + (z - za)^2)*2*(x - xa))/((x - xa)^2 + (y - ya)^2 + (z - za)^2)^4
322 // diff(Pr(x,y,z))/diff(x*z^2) = -10*n/ln(10)*(2*(x - xa)*((x - xa)^2 + (y - ya)^2 + (z - za)^2)^2 - ((x - xa)^2 + (y - ya)^2 - (z - za)^2)*2*((x - xa)^2 + (y - ya)^2 + (z - za)^2)*2*(x - xa))/((x - xa)^2 + (y - ya)^2 + (z - za)^2)^4
323 // diff(Pr(x,y,z))/diff(y^2*z) = -10*n/ln(10)*(2*(z - za)*((x - xa)^2 + (y - ya)^2 + (z - za)^2)^2 - ((x - xa)^2 - (y - ya)^2 + (z - za)^2)*2*((x - xa)^2 + (y - ya)^2 + (z - za)^2)*2*(z - za))/((x - xa)^2 + (y - ya)^2 + (z - za)^2)^4
324 // diff(Pr(x,y,z))/diff(y*z^2) = -10*n/ln(10)*(2*(y - ya)*((x - xa)^2 + (y - ya)^2 + (z - za)^2)^2 - ((x - xa)^2 + (y - ya)^2 - (z - za)^2)*2*((x - xa)^2 + (y - ya)^2 + (z - za)^2)*2*(y - ya))/((x - xa)^2 + (y - ya)^2 + (z - za)^2)^4
325 // diff(Pr(x,y,z))/diff(x*y*z) = 20*n/ln(10)*(-(x - xa)*(y - ya)*2*((x - xa)^2 + (y - ya)^2 + (z - za)^2)*2*(z - za))/((x - xa)^2 + (y - ya)^2 + (z - za)^2)^4
326
327 // evaluating at p1 = (x1, y1, z1), we get:
328 // diff(Pr(p1))/diff(x^3) = -10*n/ln(10)*(-2*(x1 - xa)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - ((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)*2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*2*(x1 - xa))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
329 // diff(Pr(p1))/diff(y^2) = -10*n/ln(10)*(-2*(y1 - ya)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - ((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)*2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*2*(y1 - ya))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
330 // diff(Pr(p1))/diff(z^3) = -10*n/ln(10)*(-2*(z1 - za)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - ((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)*2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*2*(z1 - za))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
331 // diff(Pr(p1))/diff(x^2*y) = -10*n/ln(10)*(2*(y1 - ya)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - ((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)*2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*2*(y1 - ya))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
332 // diff(Pr(p1))/diff(x^2*z) = -10*n/ln(10)*(2*(z1 - za)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - ((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)*2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*2*(z1 - za))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
333 // diff(Pr(p1))/diff(x*y^2) = -10*n/ln(10)*(2*(x1 - xa)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - ((x1 - xa)^2 - (y1 - ya)^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
334 // diff(Pr(p1))/diff(x*z^2) = -10*n/ln(10)*(2*(x1 - xa)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - ((x1 - xa)^2 + (y1 - ya)^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
335 // diff(Pr(p1))/diff(y^2*z) = -10*n/ln(10)*(2*(z1 - za)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - ((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)*2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*2*(z1 - za))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
336 // diff(Pr(p1))/diff(y*z^2) = -10*n/ln(10)*(2*(y1 - ya)*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^2 - ((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)*2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*2*(y1 - ya))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
337 // diff(Pr(p1))/diff(x*y*z) = 20*n/ln(10)*(-(x1 - xa)*(y1 - ya)*2*((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)*2*(z1 - za))/((x1 - xa)^2 + (y1 - ya)^2 + (z1 - za)^2)^4
338
339 // and substituting the distance between fingerprint and radio source d1a, we get:
340 // diff(Pr(p1))/diff(x^3) = -10*n/ln(10)*(-2*(x1 - xa)*d1a^4 - ((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)*4*d1a^2*(x1 - xa))/d1a^8
341 // diff(Pr(p1))/diff(y^3) = -10*n/ln(10)*(-2*(y1 - ya)*d1a^4 - ((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)*4*d1a^2*(y1 - ya))/d1a^8
342 // diff(Pr(p1))/diff(z^3) = -10*n/ln(10)*(-2*(z1 - za)*d1a^4 - ((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)*4*d1a^2*(z1 - za))/d1a^8
343 // diff(Pr(p1))/diff(x^2*y) = -10*n/ln(10)*(2*(y1 - ya)*d1a^4 - ((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)*4*d1a^2*(y1 - ya))/d1a^8
344 // diff(Pr(p1))/diff(x^2*z) = -10*n/ln(10)*(2*(z1 - za)*d1a^4 - ((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)*4*d1a^2*(z1 - za))/d1a^8
345 // diff(Pr(p1))/diff(x*y^2) = -10*n/ln(10)*(2*(x1 - xa)*d1a^4 - ((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)*4*d1a^2*(x1 - xa))/d1a^8
346 // diff(Pr(p1))/diff(x*z^2) = -10*n/ln(10)*(2*(x1 - xa)*d1a^4 - ((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)*4*d1a^2*(x1 - xa))/d1a^8
347 // diff(Pr(p1))/diff(y^2*z) = -10*n/ln(10)*(2*(z1 - za)*d1a^4 - ((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)*4*d1a^2*(z1 - za))/d1a^8
348 // diff(Pr(p1))/diff(y*z^2) = -10*n/ln(10)*(2*(y1 - ya)*d1a^4 - ((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)*4*d1a^2*(y1 - ya))/d1a^8
349 // diff(Pr(p1))/diff(x*y*z) = -80*n/ln(10)*((x1 - xa)*(y1 - ya)*(z1 - za)*d1a^2)/d1a^8
350
351 // Hence, the third order Taylor expansion can be expressed as:
352 // Pr(pi = (xi,yi)) = Pr(p1) +
353 // diff(Pr(p1))/diff(x)*(xi - x1) +
354 // diff(Pr(p1))/diff(y)*(yi - y1) +
355 // diff(Pr(p1))/diff(z)*(zi - z1) +
356 // 1/2*(diff(Pr(p1))/diff(x^2)*(xi - x1)^2 +
357 // diff(Pr(p1))/diff(y^2)*(yi - y1)^2 +
358 // diff(Pr(p1))/diff(z^2)*(zi - z1)^2 +
359 // 2*diff(Pr(p1))/diff(x*y)*(xi - x1)*(yi - y1)) +
360 // 2*diff(Pr(p1))/diff(y*z)*(yi - y1)*(zi - z1) +
361 // 2*diff(Pr(p1))/diff(x*z)*(xi - x1)*(zi - z1)
362 // 1/6*(diff(Pr(p1))/diff(x^3)*(xi - x1)^3 +
363 // diff(Pr(p1))/diff(y^3)*(yi - y1)^3 +
364 // diff(Pr(p1))/diff(z^3)*(zi - z1)^3 +
365 // 3*diff(Pr(p1))/diff(x^2*y)*(xi - x1)^2*(yi - y1) +
366 // 3*diff(Pr(p1))/diff(x^2*z)*(xi - x1)^2*(zi - z1) +
367 // 3*diff(Pr(p1))/diff(x*y^2)*(xi - x1)*(yi - y1)^2 +
368 // 3*diff(Pr(p1))/diff(x*z^2)*(xi - x1)*(zi - z1)^2 +
369 // 3*diff(Pr(p1))/diff(y^2*z)*(yi - y1)^2*(zi - z1) +
370 // 3*diff(Pr(p1))/diff(y*z^2)*(yi - y1)*(zi - z1)^2 +
371 // 6*diff(Pr(p1))/diff(x*y*z)*(xi - x1)*(yi - y1)*(zi - z1)
372
373 // Pr(pi = (xi,yi)) = Pr(p1) +
374 // -10*n*(x1 - xa)/(ln(10)*d1a^2)*(xi - x1) +
375 // -10*n*(y1 - ya)/(ln(10)*d1a^2)*(yi - y1) +
376 // -10*n*(z1 - za)/(ln(10)*d1a^2)*(zi - z1) +
377 // -5*n*((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)/(ln(10)*d1a^4)*(xi - x1)^2 +
378 // -5*n*((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)/(ln(10)*d1a^4)*(yi - y1)^2 +
379 // -5*n*((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)/(ln(10)*d1a^4)*(zi - z1)^2 +
380 // 20*n*(x1 - xa)*(y1 - ya)/(ln(10)*d1a^4)*(xi - x1)*(yi - y1) +
381 // 20*n*(y1 - ya)*(z1 - za)/(ln(10)*d1a^4)*(yi - y1)*(zi - z1) +
382 // 20*n*(x1 - xa)*(z1 - za)/(ln(10)*d1a^4)*(xi - x1)*(zi - z1) +
383 // -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 +
384 // -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 +
385 // -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 +
386 // -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) +
387 // -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) +
388 // -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 +
389 // -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 +
390 // -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) +
391 // -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 +
392 // -80*n/ln(10)*((x1 - xa)*(y1 - ya)*(z1 - za)*d1a^2)/d1a^8*(xi - x1)*(yi - y1)*(zi - z1)
393
394 // The equation above can be solved using a non-linear fitter such as Levenberg-Marquardt
395
396 // This method implements received power at point pi = (xi, yi) and its derivatives
397
398 final var xi = params[0];
399 final var yi = params[1];
400 final var zi = params[2];
401
402 // received power
403 final var pr = point[0];
404
405 // fingerprint coordinates
406 final var x1 = point[1];
407 final var y1 = point[2];
408 final var z1 = point[3];
409
410 // radio source coordinates
411 final var xa = point[4];
412 final var ya = point[5];
413 final var za = point[6];
414
415 // path loss exponent
416 final var n = point[7];
417
418 final var ln10 = Math.log(10.0);
419
420 final var diffXi1 = xi - x1;
421 final var diffYi1 = yi - y1;
422 final var diffZi1 = zi - z1;
423
424 final var diffX1a = x1 - xa;
425 final var diffY1a = y1 - ya;
426 final var diffZ1a = z1 - za;
427
428 final var diffXi12 = diffXi1 * diffXi1;
429 final var diffYi12 = diffYi1 * diffYi1;
430 final var diffZi12 = diffZi1 * diffZi1;
431
432 final var diffXi13 = diffXi12 * diffXi1;
433 final var diffYi13 = diffYi12 * diffYi1;
434 final var diffZi13 = diffZi12 * diffZi1;
435
436 final var diffX1a2 = diffX1a * diffX1a;
437 final var diffY1a2 = diffY1a * diffY1a;
438 final var diffZ1a2 = diffZ1a * diffZ1a;
439
440 final var d1a2 = diffX1a2 + diffY1a2 + diffZ1a2;
441 final var d1a4 = d1a2 * d1a2;
442 final var d1a8 = d1a4 * d1a4;
443
444 final var value1 = -10.0 * n * diffX1a / (ln10 * d1a2);
445 final var value2 = -10.0 * n * diffY1a / (ln10 * d1a2);
446 final var value3 = -10.0 * n * diffZ1a / (ln10 * d1a2);
447 final var value4 = -5.0 * n * (-diffX1a2 + diffY1a2 + diffZ1a2) / (ln10 * d1a4);
448 final var value5 = -5.0 * n * (diffX1a2 - diffY1a2 + diffZ1a2) / (ln10 * d1a4);
449 final var value6 = -5.0 * n * (diffX1a2 + diffY1a2 - diffZ1a2) / (ln10 * d1a4);
450 final var value7 = 20.0 * n * diffX1a * diffY1a / (ln10 * d1a4);
451 final var value8 = 20.0 * n * diffY1a * diffZ1a / (ln10 * d1a4);
452 final var value9 = 20.0 * n * diffX1a * diffZ1a / (ln10 * d1a4);
453 final var value10 = -10.0 / 6.0 * n / ln10 * (-2.0 * diffX1a * d1a4
454 - (-diffX1a2 + diffY1a2 + diffZ1a2) * 4.0 * d1a2 * diffX1a) / d1a8;
455 final var value11 = -10.0 / 6.0 * n / ln10 * (-2.0 * diffY1a * d1a4
456 - (diffX1a2 - diffY1a2 + diffZ1a2) * 4.0 * d1a2 * diffY1a) / d1a8;
457 final var value12 = -10.0 / 6.0 * n / ln10 * (-2.0 * diffZ1a * d1a4
458 - (diffX1a2 + diffY1a2 - diffZ1a2) * 4.0 * d1a2 * diffZ1a) / d1a8;
459 final var value13 = -5.0 * n / ln10 * (2.0 * diffY1a * d1a4
460 - (-diffX1a2 + diffY1a2 + diffZ1a2) * 4.0 * d1a2 * diffY1a) / d1a8;
461 final var value14 = -5.0 * n / ln10 * (2.0 * diffZ1a * d1a4
462 - (-diffX1a2 + diffY1a2 + diffZ1a2) * 4.0 * d1a2 * diffZ1a) / d1a8;
463 final var value15 = -5.0 * n / ln10 * (2.0 * diffX1a * d1a4
464 - (diffX1a2 - diffY1a2 + diffZ1a2) * 4.0 * d1a2 * diffX1a) / d1a8;
465 final var value16 = -5.0 * n / ln10 * (2.0 * diffX1a * d1a4
466 - (diffX1a2 + diffY1a2 - diffZ1a2) * 4.0 * d1a2 * diffX1a) / d1a8;
467 final var value17 = -5.0 * n / ln10 * (2.0 * diffZ1a * d1a4
468 - (diffX1a2 - diffY1a2 + diffZ1a2) * 4.0 * d1a2 * diffZ1a) / d1a8;
469 final var value18 = -5.0 * n / ln10 * (2.0 * diffY1a * d1a4
470 - (diffX1a2 + diffY1a2 - diffZ1a2) * 4.0 * d1a2 * diffY1a) / d1a8;
471 final var value19 = -80.0 * n / ln10 * (diffX1a * diffY1a * diffZ1a * d1a2) / d1a8;
472
473 // hence:
474 // Pr(pi) = Pr(p1) +
475 // value1*(xi - x1) +
476 // value2*(yi - y1) +
477 // value3*(zi - z1) +
478 // value4*(xi - x1)^2 +
479 // value5*(yi - y1)^2 +
480 // value6*(zi - z1)^2 +
481 // value7*(xi - x1)*(yi - y1) +
482 // value8*(yi - y1)*(zi - z1) +
483 // value9*(xi - x1)*(zi - z1) +
484 // value10*(xi - x1)^3 +
485 // value11*(yi - y1)^3 +
486 // value12*(zi - z1)^3 +
487 // value13*(xi - x1)^2*(yi - y1) +
488 // value14*(xi - x1)^2*(zi - z1) +
489 // value15*(xi - x1)*(yi - y1)^2 +
490 // value16*(xi - x1)*(zi - z1)^2 +
491 // value17*(yi - y1)^2*(zi - z1) +
492 // value18*(yi - y1)*(zi - z1)^2 +
493 // value19*(xi - x1)*(yi - y1)*(zi - z1)
494
495 final var result = pr
496 + value1 * diffXi1
497 + value2 * diffYi1
498 + value3 * diffZi1
499 + value4 * diffXi12
500 + value5 * diffYi12
501 + value6 * diffZi12
502 + value7 * diffXi1 * diffYi1
503 + value8 * diffYi1 * diffZi1
504 + value9 * diffXi1 * diffZi1
505 + value10 * diffXi13
506 + value11 * diffYi13
507 + value12 * diffZi13
508 + value13 * diffXi12 * diffYi1
509 + value14 * diffXi12 * diffZi1
510 + value15 * diffXi1 * diffYi12
511 + value16 * diffXi1 * diffZi12
512 + value17 * diffYi12 * diffZi1
513 + value18 * diffYi1 * diffZi12
514 + value19 * diffXi1 * diffYi1 * diffZi1;
515
516 // derivative respect xi
517
518 // diff(Pr(pi))/diff(xi) = value1 +
519 // 2*value4*(xi - x1) +
520 // value7*(yi - y1) +
521 // value9*(zi - z1) +
522 // 3*value10*(xi - x1)^2 +
523 // 2*value13*(xi - x1)*(yi - y1) +
524 // 2*value14*(xi - x1)*(zi - z1) +
525 // value15*(yi - y1)^2 +
526 // value16*(zi - z1)^2 +
527 // value19*(yi - y1)*(zi - z1)
528
529 derivatives[0] = value1 + 2.0 * value4 * diffXi1 + value7 * diffYi1 + value9 * diffZi1
530 + 3.0 * value10 * diffXi12 + 2.0 * value13 * diffXi1 * diffYi1 + 2.0 * value14 * diffXi1 * diffZi1
531 + value15 * diffYi12 + value16 * diffZi12 + value19 * diffYi1 * diffZi1;
532
533 // derivative respect yi
534
535 // diff(Pr(pi))/diff(yi) = value2 +
536 // 2*value5*(yi - y1) +
537 // value7*(xi - x1) +
538 // value8*(zi - z1) +
539 // 3*value11*(yi - y1)^2 +
540 // value13*(xi - x1)^2 +
541 // 2*value15*(xi - x1)*(yi - y1) +
542 // 2*value17*(yi - y1)*(zi - z1) +
543 // value18*(zi - z1)^2 +
544 // value19*(xi - x1)*(zi - z1)
545
546 derivatives[1] = value2 + 2.0 * value5 * diffYi1 + value7 * diffXi1 + value8 * diffZi1
547 + 3.0 * value11 * diffYi12 + value13 * diffXi12 + 2.0 * value15 * diffXi1 * diffYi1
548 + 2.0 * value17 * diffYi1 * diffZi1 + value18 * diffZi12 + value19 * diffXi1 * diffZi1;
549
550 // derivative respect zi
551
552 // diff(Pr(pi))/diff(zi) = value3 +
553 // 2*value6*(zi - z1) +
554 // value8*(yi - y1) +
555 // value9*(xi - x1) +
556 // 3*value12*(zi - z1)^2 +
557 // value14*(xi - x1)^2 +
558 // 2*value16*(xi - x1)*(zi - z1) +
559 // value17*(yi - y1)^2 +
560 // 2*value18*(yi - y1)*(zi - z1) +
561 // value19*(xi - x1)*(yi - y1)
562
563 derivatives[2] = value3 + 2.0 * value6 * diffZi1 + value8 * diffYi1
564 + value9 * diffXi1 + 3.0 * value12 * diffZi12 + value14 * diffXi12 + 2.0 * value16
565 * diffXi1 * diffZi1 + value17 * diffYi12 + 2.0 * value18 * diffYi1 * diffZi1
566 + value19 * diffXi1 * diffYi1;
567
568 return result;
569 }
570
571 /**
572 * Propagates provided variances into RSSI variance of non-located fingerprint
573 * reading.
574 *
575 * @param fingerprintRssi closest located fingerprint reading RSSI expressed in dBm's.
576 * @param pathlossExponent path-loss exponent.
577 * @param fingerprintPosition position of closest fingerprint.
578 * @param radioSourcePosition radio source position associated to fingerprint reading.
579 * @param estimatedPosition position to be estimated. Usually this is equal to the
580 * initial position used by a non-linear algorithm.
581 * @param fingerprintRssiVariance variance of fingerprint RSSI or null if unknown.
582 * @param pathlossExponentVariance variance of path-loss exponent or null if unknown.
583 * @param fingerprintPositionCovariance covariance of fingerprint position or null if
584 * unknown.
585 * @param radioSourcePositionCovariance covariance of radio source position or null if
586 * unknown.
587 * @return variance of RSSI measured at non located fingerprint reading.
588 */
589 @Override
590 @SuppressWarnings("Duplicates")
591 protected Double propagateVariances(
592 final double fingerprintRssi, final double pathlossExponent, final Point3D fingerprintPosition,
593 final Point3D radioSourcePosition, final Point3D estimatedPosition, final Double fingerprintRssiVariance,
594 final Double pathlossExponentVariance, final Matrix fingerprintPositionCovariance,
595 final Matrix radioSourcePositionCovariance) {
596 try {
597 final var dist = Utils.propagateVariancesToRssiVarianceThirdOrderNonLinear3D(fingerprintRssi,
598 pathlossExponent, fingerprintPosition, radioSourcePosition, estimatedPosition,
599 fingerprintRssiVariance, pathlossExponentVariance, fingerprintPositionCovariance,
600 radioSourcePositionCovariance, null);
601 if (dist == null) {
602 return null;
603 }
604
605 final var covariance = dist.getCovariance();
606 if (covariance == null) {
607 return null;
608 }
609
610 return covariance.getElementAt(0, 0);
611
612 } catch (IndoorException e) {
613 return null;
614 }
615 }
616 }
617