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 * 3D 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 second-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 SecondOrderNonLinearFingerprintPositionEstimator3D extends NonLinearFingerprintPositionEstimator3D {
42
43 /**
44 * Constructor.
45 */
46 public SecondOrderNonLinearFingerprintPositionEstimator3D() {
47 }
48
49 /**
50 * Constructor.
51 *
52 * @param listener listener in charge of handling events.
53 */
54 public SecondOrderNonLinearFingerprintPositionEstimator3D(
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 SecondOrderNonLinearFingerprintPositionEstimator3D(
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 SecondOrderNonLinearFingerprintPositionEstimator3D(
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 SecondOrderNonLinearFingerprintPositionEstimator3D(
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 2D position estimation at least 2
137 * located total readings are required among all fingerprints, for example 2
138 * readings are required in a single fingerprint, or at least 2 fingerprints at
139 * different locations containing a single reading are required. For 3D position
140 * estimation 3 located total readings are required among all fingerprints).
141 */
142 public SecondOrderNonLinearFingerprintPositionEstimator3D(
143 final List<? extends RssiFingerprintLocated<? extends RadioSource,
144 ? extends RssiReading<? extends RadioSource>, Point3D>> locatedFingerprints,
145 final RssiFingerprint<? extends RadioSource,
146 ? extends RssiReading<? extends RadioSource>> fingerprint,
147 final List<? extends RadioSourceLocated<Point3D>> sources, Point3D initialPosition,
148 final FingerprintPositionEstimatorListener<Point3D> listener) {
149 super(locatedFingerprints, fingerprint, sources, initialPosition, listener);
150 }
151
152 /**
153 * Gets type of position estimator.
154 *
155 * @return type of position estimator.
156 */
157 @Override
158 public NonLinearFingerprintPositionEstimatorType getType() {
159 return NonLinearFingerprintPositionEstimatorType.SECOND_ORDER;
160 }
161
162 /**
163 * Evaluates a non-linear multi dimension function at provided point using
164 * provided parameters and returns its evaluation and derivatives of the
165 * function respect the function parameters.
166 *
167 * @param i number of sample being evaluated.
168 * @param point point where function will be evaluated.
169 * @param params initial parameters estimation to be tried. These will
170 * change as the Levenberg-Marquardt algorithm iterates to the best solution.
171 * These are used as input parameters along with point to evaluate function.
172 * @param derivatives partial derivatives of the function respect to each
173 * provided parameter.
174 * @return function evaluation at provided point.
175 */
176 @Override
177 @SuppressWarnings("Duplicates")
178 protected double evaluate(
179 final int i, final double[] point, final double[] params, final double[] derivatives) {
180 // This method implements received power at point pi = (xi, yi, zi) and its derivatives
181
182 // Pr(pi) = Pr(p1)
183 // - 10*n*(x1 - xa)/(ln(10)*d1a^2)*(xi - x1)
184 // - 10*n*(y1 - ya)/(ln(10)*d1a^2)*(yi - y1)
185 // - 10*n*(z1 - za)/(ln(10)*d1a^2)*(zi - z1)
186 // - 5*n*((y1 - ya)^2 + (z1 - za)^2 - (x1 - xa)^2)/(ln(10)*d1a^4)*(xi - x1)^2
187 // - 5*n*((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)/(ln(10)*d1a^4)*(yi - y1)^2
188 // - 5*n*((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)/(ln(10)*d1a^4)*(zi - z1)^2
189 // + 20*n*(x1 - xa)*(y1 - ya)/(ln(10)*d1a^4)*(xi - x1)*(yi - y1)
190 // + 20*n*(y1 - ya)*(z1 - za)/(ln(10)*d1a^4)*(yi - y1)*(zi - z1)
191 // + 20*n*(x1 - xa)*(z1 - za)/(ln(10)*d1a^4)*(xi - x1)*(zi - z1)
192
193 final var xi = params[0];
194 final var yi = params[1];
195 final var zi = params[2];
196
197 // received power
198 final var pr = point[0];
199
200 // fingerprint coordinates
201 final var x1 = point[1];
202 final var y1 = point[2];
203 final var z1 = point[3];
204
205 // radio source coordinates
206 final var xa = point[4];
207 final var ya = point[5];
208 final var za = point[6];
209
210 // path loss exponent
211 final var n = point[7];
212
213 final var ln10 = Math.log(10.0);
214
215 final var diffXi1 = xi - x1;
216 final var diffYi1 = yi - y1;
217 final var diffZi1 = zi - z1;
218
219 final var diffX1a = x1 - xa;
220 final var diffY1a = y1 - ya;
221 final var diffZ1a = z1 - za;
222
223 final var diffXi12 = diffXi1 * diffXi1;
224 final var diffYi12 = diffYi1 * diffYi1;
225 final var diffZi12 = diffZi1 * diffZi1;
226
227 final var diffX1a2 = diffX1a * diffX1a;
228 final var diffY1a2 = diffY1a * diffY1a;
229 final var diffZ1a2 = diffZ1a * diffZ1a;
230
231 final var d1a2 = diffX1a2 + diffY1a2 + diffZ1a2;
232 final var d1a4 = d1a2 * d1a2;
233
234 final var value1 = -10.0 * n * diffX1a / (ln10 * d1a2);
235 final var value2 = -10.0 * n * diffY1a / (ln10 * d1a2);
236 final var value3 = -10.0 * n * diffZ1a / (ln10 * d1a2);
237 final var value4 = -5.0 * n * (-diffX1a2 + diffY1a2 + diffZ1a2) / (ln10 * d1a4);
238 final var value5 = -5.0 * n * (diffX1a2 - diffY1a2 + diffZ1a2) / (ln10 * d1a4);
239 final var value6 = -5.0 * n * (diffX1a2 + diffY1a2 - diffZ1a2) / (ln10 * d1a4);
240 final var value7 = 20.0 * n * diffX1a * diffY1a / (ln10 * d1a4);
241 final var value8 = 20.0 * n * diffY1a * diffZ1a / (ln10 * d1a4);
242 final var value9 = 20.0 * n * diffX1a * diffZ1a / (ln10 * d1a4);
243
244 final var result = pr
245 + value1 * diffXi1
246 + value2 * diffYi1
247 + value3 * diffZi1
248 + value4 * diffXi12
249 + value5 * diffYi12
250 + value6 * diffZi12
251 + value7 * diffXi1 * diffYi1
252 + value8 * diffYi1 * diffZi1
253 + value9 * diffXi1 * diffZi1;
254
255 // derivative respect xi
256 // diff(Pr(pi))/diff(xi) = - 10*n*(x1 - xa)/(ln(10)*d1a^2)
257 // - 10*n*((y1 - ya)^2 + (z1 - za)^2) - (x1 - xa)^2)/(ln(10)*d1a^4)*(xi - x1)
258 // + 20*n*(x1 - xa)*(y1 - ya)/(ln(10)*d1a^4)*(yi - y1)
259 // + 20*n*(x1 - xa)*(z1 - za)/(ln(10)*d1a^4)*(zi - z1)
260 derivatives[0] = value1 + 2.0 * value4 * diffXi1 + value7 * diffYi1 + value8 * diffZi1;
261
262 // derivative respect yi
263 // diff(Pr(pi))/diff(yi) = - 10*n*(y1 - ya)/(ln(10)*d1a^2)
264 // - 10*n*((x1 - xa)^2 - (y1 - ya)^2 + (z1 - za)^2)/(ln(10)*d1a^4)*(yi - y1)
265 // + 20*n*(x1 - xa)*(y1 - ya)/(ln(10)*d1a^4)*(xi - x1)
266 // + 20*n*(y1 - ya)*(z1 - za)/(ln(10)*d1a^4)*(zi - z1)
267 derivatives[1] = value2 + 2.0 * value5 * diffYi1 + value7 * diffXi1 + value8 * diffZi1;
268
269 // derivative respect zi
270 // diff(Pr(pi))/diff(zi) = - 10*n*(z1 - za)/(ln(10)*d1a^2)
271 // - 10*n*((x1 - xa)^2 + (y1 - ya)^2 - (z1 - za)^2)/(ln(10)*d1a^4)*(zi - z1)
272 // + 20*n*(y1 - ya)*(z1 - za)/(ln(10)*d1a^4)*(yi - y1)
273 // + 20*n*(x1 - xa)*(z1 - za)/(ln(10)*d1a^4)*(xi - x1)
274 derivatives[2] = value3 + 2.0 * value6 * diffZi1 + value8 * diffYi1 + value9 * diffXi1;
275
276 return result;
277 }
278
279 /**
280 * Propagates provided variances into RSSI variance of non-located fingerprint
281 * reading.
282 *
283 * @param fingerprintRssi closest located fingerprint reading RSSI expressed in dBm's.
284 * @param pathlossExponent path-loss exponent.
285 * @param fingerprintPosition position of closest fingerprint.
286 * @param radioSourcePosition radio source position associated to fingerprint reading.
287 * @param estimatedPosition position to be estimated. Usually this is equal to the
288 * initial position used by a non-linear algorithm.
289 * @param fingerprintRssiVariance variance of fingerprint RSSI or null if unknown.
290 * @param pathlossExponentVariance variance of path-loss exponent or null if unknown.
291 * @param fingerprintPositionCovariance covariance of fingerprint position or null if
292 * unknown.
293 * @param radioSourcePositionCovariance covariance of radio source position or null if
294 * unknown.
295 * @return variance of RSSI measured at non located fingerprint reading.
296 */
297 @Override
298 @SuppressWarnings("Duplicates")
299 protected Double propagateVariances(
300 final double fingerprintRssi, final double pathlossExponent, final Point3D fingerprintPosition,
301 final Point3D radioSourcePosition, final Point3D estimatedPosition, final Double fingerprintRssiVariance,
302 final Double pathlossExponentVariance, final Matrix fingerprintPositionCovariance,
303 final Matrix radioSourcePositionCovariance) {
304 try {
305 final var dist = Utils.propagateVariancesToRssiVarianceSecondOrderNonLinear3D(fingerprintRssi,
306 pathlossExponent, fingerprintPosition, radioSourcePosition, estimatedPosition,
307 fingerprintRssiVariance, pathlossExponentVariance, fingerprintPositionCovariance,
308 radioSourcePositionCovariance, null);
309 if (dist == null) {
310 return null;
311 }
312
313 final var covariance = dist.getCovariance();
314 if (covariance == null) {
315 return null;
316 }
317
318 return covariance.getElementAt(0, 0);
319
320 } catch (IndoorException e) {
321 return null;
322 }
323 }
324 }