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 import com.irurueta.statistics.MultivariateNormalDist;
28
29 import java.util.List;
30
31 /**
32 * 3D position estimator based on located fingerprints containing only RSSI readings and
33 * having as well prior knowledge of the location of radio sources associated to those
34 * readings.
35 * This implementation uses a first-order Taylor approximation over provided located
36 * fingerprints to determine an approximate position for a non-located fingerprint using
37 * a non-linear solving algorithm.
38 * An initial position can be provided as a starting point to solve the position,
39 * otherwise the average point of selected nearest fingerprints is used as a starting
40 * point.
41 */
42 public class FirstOrderNonLinearFingerprintPositionEstimator3D extends NonLinearFingerprintPositionEstimator3D {
43
44 /**
45 * Constructor.
46 */
47 public FirstOrderNonLinearFingerprintPositionEstimator3D() {
48 }
49
50 /**
51 * Constructor.
52 *
53 * @param listener listener in charge of handling events.
54 */
55 public FirstOrderNonLinearFingerprintPositionEstimator3D(
56 final FingerprintPositionEstimatorListener<Point3D> listener) {
57 super(listener);
58 }
59
60 /**
61 * Constructor.
62 *
63 * @param locatedFingerprints located fingerprints containing RSSI readings.
64 * @param fingerprint fingerprint containing readings at an unknown location
65 * for provided located fingerprints.
66 * @param sources located radio sources.
67 * @throws IllegalArgumentException if provided non located fingerprint is null,
68 * located fingerprints value is null or there are not enough fingerprints or
69 * readings within provided fingerprints (for 3D position estimation 3 located
70 * total readings are required among all fingerprints).
71 */
72 public FirstOrderNonLinearFingerprintPositionEstimator3D(
73 final List<? extends RssiFingerprintLocated<? extends RadioSource,
74 ? extends RssiReading<? extends RadioSource>, Point3D>> locatedFingerprints,
75 final RssiFingerprint<? extends RadioSource,
76 ? extends RssiReading<? extends RadioSource>> fingerprint,
77 final List<? extends RadioSourceLocated<Point3D>> sources) {
78 super(locatedFingerprints, fingerprint, sources);
79 }
80
81 /**
82 * Constructor.
83 *
84 * @param locatedFingerprints located fingerprints containing RSSI readings.
85 * @param fingerprint fingerprint containing readings at an unknown location
86 * for provided located fingerprints.
87 * @param sources located radio sources.
88 * @param listener listener in charge of handling events.
89 * @throws IllegalArgumentException if provided non located fingerprint is null,
90 * located fingerprints value is null or there are not enough fingerprints or
91 * readings within provided fingerprints (for 3D position estimation 3 located
92 * total readings are required among all fingerprints).
93 */
94 public FirstOrderNonLinearFingerprintPositionEstimator3D(
95 final List<? extends RssiFingerprintLocated<? extends RadioSource,
96 ? extends RssiReading<? extends RadioSource>, Point3D>> locatedFingerprints,
97 final RssiFingerprint<? extends RadioSource,
98 ? extends RssiReading<? extends RadioSource>> fingerprint,
99 final List<? extends RadioSourceLocated<Point3D>> sources,
100 final FingerprintPositionEstimatorListener<Point3D> listener) {
101 super(locatedFingerprints, fingerprint, sources, listener);
102 }
103
104 /**
105 * Constructor.
106 *
107 * @param locatedFingerprints located fingerprints containing RSSI readings.
108 * @param fingerprint fingerprint containing readings at an unknown location
109 * for provided located fingerprints.
110 * @param sources located radio sources.
111 * @param initialPosition initial position to start the solving algorithm or null.
112 * @throws IllegalArgumentException if provided non located fingerprint is null,
113 * located fingerprints value is null or there are not enough fingerprints or
114 * readings within provided fingerprints (for 3D position estimation 3 located
115 * total readings are required among all fingerprints).
116 */
117 public FirstOrderNonLinearFingerprintPositionEstimator3D(
118 final List<? extends RssiFingerprintLocated<? extends RadioSource,
119 ? extends RssiReading<? extends RadioSource>, Point3D>> locatedFingerprints,
120 final RssiFingerprint<? extends RadioSource,
121 ? extends RssiReading<? extends RadioSource>> fingerprint,
122 final List<? extends RadioSourceLocated<Point3D>> sources, Point3D initialPosition) {
123 super(locatedFingerprints, fingerprint, sources, initialPosition);
124 }
125
126 /**
127 * Constructor.
128 *
129 * @param locatedFingerprints located fingerprints containing RSSI readings.
130 * @param fingerprint fingerprint containing readings at an unknown location
131 * for provided located fingerprints.
132 * @param sources located radio sources.
133 * @param initialPosition initial position to start the solving algorithm or null.
134 * @param listener listener in charge of handling events.
135 * @throws IllegalArgumentException if provided non located fingerprint is null,
136 * located fingerprints value is null or there are not enough fingerprints or
137 * readings within provided fingerprints (for 2D position estimation at least 2
138 * located total readings are required among all fingerprints, for example 2
139 * readings are required in a single fingerprint, or at least 2 fingerprints at
140 * different locations containing a single reading are required. For 3D position
141 * estimation 3 located total readings are required among all fingerprints).
142 */
143 public FirstOrderNonLinearFingerprintPositionEstimator3D(
144 final List<? extends RssiFingerprintLocated<? extends RadioSource,
145 ? extends RssiReading<? extends RadioSource>, Point3D>> locatedFingerprints,
146 final RssiFingerprint<? extends RadioSource,
147 ? extends RssiReading<? extends RadioSource>> fingerprint,
148 final List<? extends RadioSourceLocated<Point3D>> sources, Point3D initialPosition,
149 final FingerprintPositionEstimatorListener<Point3D> listener) {
150 super(locatedFingerprints, fingerprint, sources, initialPosition, listener);
151 }
152
153 /**
154 * Gets type of position estimator.
155 *
156 * @return type of position estimator.
157 */
158 @Override
159 public NonLinearFingerprintPositionEstimatorType getType() {
160 return NonLinearFingerprintPositionEstimatorType.FIRST_ORDER;
161 }
162
163 /**
164 * Evaluates a non-linear multi dimension function at provided point using
165 * provided parameters and returns its evaluation and derivatives of the
166 * function respect the function parameters.
167 *
168 * @param i number of sample being evaluated.
169 * @param point point where function will be evaluated.
170 * @param params initial parameters estimation to be tried. These will
171 * change as the Levenberg-Marquardt algorithm iterates to the best solution.
172 * These are used as input parameters along with point to evaluate function.
173 * @param derivatives partial derivatives of the function respect to each
174 * provided parameter.
175 * @return function evaluation at provided point.
176 */
177 @Override
178 @SuppressWarnings("Duplicates")
179 protected double evaluate(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
187 final var xi = params[0];
188 final var yi = params[1];
189 final var zi = params[2];
190
191 // received power
192 final var pr = point[0];
193
194 // fingerprint coordinates
195 final var x1 = point[1];
196 final var y1 = point[2];
197 final var z1 = point[3];
198
199 //radio source coordinates
200 final var xa = point[4];
201 final var ya = point[5];
202 final var za = point[6];
203
204 // path loss exponent
205 final var n = point[7];
206
207 final var ln10 = Math.log(10.0);
208
209 final var diffXi1 = xi - x1;
210 final var diffYi1 = yi - y1;
211 final var diffZi1 = zi - z1;
212
213 final var diffX1a = x1 - xa;
214 final var diffY1a = y1 - ya;
215 final var diffZ1a = z1 - za;
216
217 final var diffX1a2 = diffX1a * diffX1a;
218 final var diffY1a2 = diffY1a * diffY1a;
219 final var diffZ1a2 = diffZ1a * diffZ1a;
220
221 final var d1a2 = diffX1a2 + diffY1a2 + diffZ1a2;
222
223 final var value1 = -10.0 * n * diffX1a / (ln10 * d1a2);
224 final var value2 = -10.0 * n * diffY1a / (ln10 * d1a2);
225 final var value3 = -10.0 * n * diffZ1a / (ln10 * d1a2);
226
227 final var result = pr + value1 * diffXi1 + value2 * diffYi1 + value3 * diffZi1;
228
229 // derivative respect xi
230 // diff(Pr(pi))/diff(xi) = - 10*n*(x1 - xa)/(ln(10)*d1a^2)
231 derivatives[0] = value1;
232
233 // derivative respect yi
234 // diff(Pr(pi))/diff(yi) = - 10*n*(y1 - ya)/(ln(10)*d1a^2)
235 derivatives[1] = value2;
236
237 // derivative respect zi
238 // diff(Pr(pi))/diff(zi) = - 10*n*(z1 - za)/(ln(10)*d1a^2)
239 derivatives[2] = value3;
240
241 return result;
242 }
243
244 /**
245 * Propagates provided variances into RSSI variance of non-located fingerprint
246 * reading.
247 *
248 * @param fingerprintRssi closest located fingerprint reading RSSI expressed in dBm's.
249 * @param pathlossExponent path-loss exponent.
250 * @param fingerprintPosition position of closest fingerprint.
251 * @param radioSourcePosition radio source position associated to fingerprint reading.
252 * @param estimatedPosition position to be estimated. Usually this is equal to the
253 * initial position used by a non-linear algorithm.
254 * @param fingerprintRssiVariance variance of fingerprint RSSI or null if unknown.
255 * @param pathlossExponentVariance variance of path-loss exponent or null if unknown.
256 * @param fingerprintPositionCovariance covariance of fingerprint position or null if
257 * unknown.
258 * @param radioSourcePositionCovariance covariance of radio source position or null if
259 * unknown.
260 * @return variance of RSSI measured at non located fingerprint reading.
261 */
262 @Override
263 @SuppressWarnings("Duplicates")
264 protected Double propagateVariances(
265 final double fingerprintRssi, final double pathlossExponent,
266 final Point3D fingerprintPosition, final Point3D radioSourcePosition,
267 final Point3D estimatedPosition, final Double fingerprintRssiVariance,
268 final Double pathlossExponentVariance,
269 final Matrix fingerprintPositionCovariance,
270 final Matrix radioSourcePositionCovariance) {
271 try {
272 final var dist = Utils.propagateVariancesToRssiVarianceFirstOrderNonLinear3D(fingerprintRssi,
273 pathlossExponent, fingerprintPosition, radioSourcePosition, estimatedPosition,
274 fingerprintRssiVariance, pathlossExponentVariance, fingerprintPositionCovariance,
275 radioSourcePositionCovariance, null);
276 if (dist == null) {
277 return null;
278 }
279
280 final var covariance = dist.getCovariance();
281 if (covariance == null) {
282 return null;
283 }
284
285 return covariance.getElementAt(0, 0);
286
287 } catch (IndoorException e) {
288 return null;
289 }
290 }
291 }