1 /*
2 * Copyright (C) 2018 Alberto Irurueta Carro (alberto@irurueta.com)
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16 package com.irurueta.navigation.indoor.fingerprint;
17
18 import com.irurueta.algebra.Matrix;
19 import com.irurueta.geometry.Point2D;
20 import com.irurueta.navigation.indoor.IndoorException;
21 import com.irurueta.navigation.indoor.RadioSource;
22 import com.irurueta.navigation.indoor.RadioSourceLocated;
23 import com.irurueta.navigation.indoor.RssiFingerprint;
24 import com.irurueta.navigation.indoor.RssiFingerprintLocated;
25 import com.irurueta.navigation.indoor.RssiReading;
26 import com.irurueta.navigation.indoor.Utils;
27
28 import java.util.List;
29
30 /**
31 * 2D position estimator based on located fingerprints containing only RSSI readings and
32 * having as well prior knowledge of the location of radio sources associated to those
33 * readings.
34 * This implementation uses a 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 SecondOrderNonLinearFingerprintPositionEstimator2D extends NonLinearFingerprintPositionEstimator2D {
42
43 /**
44 * Constructor.
45 */
46 public SecondOrderNonLinearFingerprintPositionEstimator2D() {
47 }
48
49 /**
50 * Constructor.
51 *
52 * @param listener listener in charge of handling events.
53 */
54 public SecondOrderNonLinearFingerprintPositionEstimator2D(
55 final FingerprintPositionEstimatorListener<Point2D> listener) {
56 super(listener);
57 }
58
59 /**
60 * Constructor.
61 *
62 * @param locatedFingerprints located fingerprints containing RSSI readings.
63 * @param fingerprint fingerprint containing readings at an unknown location
64 * for provided located fingerprints.
65 * @param sources located radio sources.
66 * @throws IllegalArgumentException if provided non located fingerprint is null,
67 * located fingerprints value is null or there are not enough fingerprints or
68 * readings within provided fingerprints (for 2D position estimation at least 2
69 * located total readings are required among all fingerprints, for example 2
70 * readings are required in a single fingerprint, or at least 2 fingerprints at
71 * different locations containing a single reading are required).
72 */
73 public SecondOrderNonLinearFingerprintPositionEstimator2D(
74 final List<? extends RssiFingerprintLocated<? extends RadioSource,
75 ? extends RssiReading<? extends RadioSource>, Point2D>> locatedFingerprints,
76 final RssiFingerprint<? extends RadioSource,
77 ? extends RssiReading<? extends RadioSource>> fingerprint,
78 final List<? extends RadioSourceLocated<Point2D>> sources) {
79 super(locatedFingerprints, fingerprint, sources);
80 }
81
82 /**
83 * Constructor.
84 *
85 * @param locatedFingerprints located fingerprints containing RSSI readings.
86 * @param fingerprint fingerprint containing readings at an unknown location
87 * for provided located fingerprints.
88 * @param sources located radio sources.
89 * @param listener listener in charge of handling events.
90 * @throws IllegalArgumentException if provided non located fingerprint is null,
91 * located fingerprints value is null or there are not enough fingerprints or
92 * readings within provided fingerprints (for 2D position estimation at least 2
93 * located total readings are required among all fingerprints, for example 2
94 * readings are required in a single fingerprint, or at least 2 fingerprints at
95 * different locations containing a single reading are required).
96 */
97 public SecondOrderNonLinearFingerprintPositionEstimator2D(
98 final List<? extends RssiFingerprintLocated<? extends RadioSource,
99 ? extends RssiReading<? extends RadioSource>, Point2D>> locatedFingerprints,
100 final RssiFingerprint<? extends RadioSource,
101 ? extends RssiReading<? extends RadioSource>> fingerprint,
102 final List<? extends RadioSourceLocated<Point2D>> sources,
103 final FingerprintPositionEstimatorListener<Point2D> listener) {
104 super(locatedFingerprints, fingerprint, sources, listener);
105 }
106
107 /**
108 * Constructor.
109 *
110 * @param locatedFingerprints located fingerprints containing RSSI readings.
111 * @param fingerprint fingerprint containing readings at an unknown location
112 * for provided located fingerprints.
113 * @param sources located radio sources.
114 * @param initialPosition initial position to start the solving algorithm or null.
115 * @throws IllegalArgumentException if provided non located fingerprint is null,
116 * located fingerprints value is null or there are not enough fingerprints or
117 * readings within provided fingerprints (for 2D position estimation at least 2
118 * located total readings are required among all fingerprints, for example 2
119 * readings are required in a single fingerprint, or at least 2 fingerprints at
120 * different locations containing a single reading are required).
121 */
122 public SecondOrderNonLinearFingerprintPositionEstimator2D(
123 final List<? extends RssiFingerprintLocated<? extends RadioSource,
124 ? extends RssiReading<? extends RadioSource>, Point2D>> locatedFingerprints,
125 final RssiFingerprint<? extends RadioSource,
126 ? extends RssiReading<? extends RadioSource>> fingerprint,
127 final List<? extends RadioSourceLocated<Point2D>> sources, Point2D initialPosition) {
128 super(locatedFingerprints, fingerprint, sources, initialPosition);
129 }
130
131 /**
132 * Constructor.
133 *
134 * @param locatedFingerprints located fingerprints containing RSSI readings.
135 * @param fingerprint fingerprint containing readings at an unknown location
136 * for provided located fingerprints.
137 * @param sources located radio sources.
138 * @param initialPosition initial position to start the solving algorithm or null.
139 * @param listener listener in charge of handling events.
140 * @throws IllegalArgumentException if provided non located fingerprint is null,
141 * located fingerprints value is null or there are not enough fingerprints or
142 * readings within provided fingerprints (for 2D position estimation at least 2
143 * located total readings are required among all fingerprints, for example 2
144 * readings are required in a single fingerprint, or at least 2 fingerprints at
145 * different locations containing a single reading are required).
146 */
147 public SecondOrderNonLinearFingerprintPositionEstimator2D(
148 final List<? extends RssiFingerprintLocated<? extends RadioSource,
149 ? extends RssiReading<? extends RadioSource>, Point2D>> locatedFingerprints,
150 final RssiFingerprint<? extends RadioSource,
151 ? extends RssiReading<? extends RadioSource>> fingerprint,
152 final List<? extends RadioSourceLocated<Point2D>> sources, Point2D initialPosition,
153 final FingerprintPositionEstimatorListener<Point2D> listener) {
154 super(locatedFingerprints, fingerprint, sources, initialPosition, listener);
155 }
156
157 /**
158 * Gets type of position estimator.
159 *
160 * @return type of position estimator.
161 */
162 @Override
163 public NonLinearFingerprintPositionEstimatorType getType() {
164 return NonLinearFingerprintPositionEstimatorType.SECOND_ORDER;
165 }
166
167 /**
168 * Evaluates a non-linear multi dimension function at provided point using
169 * provided parameters and returns its evaluation and derivatives of the
170 * function respect the function parameters.
171 *
172 * @param i number of sample being evaluated.
173 * @param point point where function will be evaluated.
174 * @param params initial parameters estimation to be tried. These will
175 * change as the Levenberg-Marquardt algorithm iterates to the best solution.
176 * These are used as input parameters along with point to evaluate function.
177 * @param derivatives partial derivatives of the function respect to each
178 * provided parameter.
179 * @return function evaluation at provided point.
180 */
181 @Override
182 @SuppressWarnings("Duplicates")
183 protected double evaluate(
184 final int i, final double[] point, final double[] params, final double[] derivatives) {
185 // This method implements received power at point pi = (xi, yi) and its derivatives
186
187 // Pr(pi) = Pr(p1)
188 // - 10*n*(x1 - xa)/(ln(10)*d1a^2)*(xi - x1)
189 // - 10*n*(y1 - ya)/(ln(10)*d1a^2)*(yi - y1)
190 // - 5*n*((y1 - ya)^2 - (x1 - xa)^2)/(ln(10)*d1a^4)*(xi - x1)^2
191 // - 5*n*((x1 - xa)^2 - (y1 - ya)^2)/(ln(10)*d1a^4)*(yi - y1)^2
192 // + 20*n*(x1 - xa)*(y1 - ya)/(ln(10)*d1a^4))*(xi - x1)*(yi - y1)
193
194 final var xi = params[0];
195 final var yi = params[1];
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
204 // radio source coordinates
205 final var xa = point[3];
206 final var ya = point[4];
207
208 // path loss exponent
209 final var n = point[5];
210
211 final var ln10 = Math.log(10.0);
212
213 final var diffXi1 = xi - x1;
214 final var diffYi1 = yi - y1;
215
216 final var diffX1a = x1 - xa;
217 final var diffY1a = y1 - ya;
218
219 final var diffXi12 = diffXi1 * diffXi1;
220 final var diffYi12 = diffYi1 * diffYi1;
221
222 final var diffX1a2 = diffX1a * diffX1a;
223 final var diffY1a2 = diffY1a * diffY1a;
224
225 final var d1a2 = diffX1a2 + diffY1a2;
226 final var d1a4 = d1a2 * d1a2;
227
228 final var value1 = -10.0 * n * diffX1a / (ln10 * d1a2);
229 final var value2 = -10.0 * n * diffY1a / (ln10 * d1a2);
230 final var value3 = -5.0 * n * (-diffX1a2 + diffY1a2) / (ln10 * d1a4);
231 final var value4 = -5.0 * n * (diffX1a2 - diffY1a2) / (ln10 * d1a4);
232 final var value5 = 20.0 * n * diffX1a * diffY1a / (ln10 * d1a4);
233
234 final var result = pr
235 + value1 * diffXi1
236 + value2 * diffYi1
237 + value3 * diffXi12
238 + value4 * diffYi12
239 + value5 * diffXi1 * diffYi1;
240
241 // derivative respect xi
242 // diff(Pr(pi))/diff(xi) = - 10*n*(x1 - xa)/(ln(10)*d1a^2)
243 // - 10*n*((y1 - ya)^2 - (x1 - xa)^2)/(ln(10)*d1a^4)*(xi - x1)
244 // + 20*n*(x1 - xa)*(y1 - ya)/(ln(10)*d1a^4)*(yi - y1)
245 derivatives[0] = value1 + 2.0 * value3 * diffXi1 + value5 * diffYi1;
246
247 // derivative respect yi
248 // diff(Pr(pi))/diff(yi) = - 10*n*(y1 - ya)/(ln(10)*d1a^2)
249 // - 10*n*((x1 - xa)^2 - (y1 - ya)^2)/(ln(10)*d1a^4)*(yi - y1)
250 // + 20*n*(x1 - xa)*(y1 - ya)/(ln(10)*d1a^4)*(xi - x1)
251 derivatives[1] = value2 + 2.0 * value4 * diffYi1 + value5 * diffXi1;
252
253 return result;
254 }
255
256 /**
257 * Propagates provided variances into RSSI variance of non-located fingerprint
258 * reading.
259 *
260 * @param fingerprintRssi closest located fingerprint reading RSSI expressed in dBm's.
261 * @param pathlossExponent path-loss exponent.
262 * @param fingerprintPosition position of closest fingerprint.
263 * @param radioSourcePosition radio source position associated to fingerprint reading.
264 * @param estimatedPosition position to be estimated. Usually this is equal to the
265 * initial position used by a non-linear algorithm.
266 * @param fingerprintRssiVariance variance of fingerprint RSSI or null if unknown.
267 * @param pathlossExponentVariance variance of path-loss exponent or null if unknown.
268 * @param fingerprintPositionCovariance covariance of fingerprint position or null if
269 * unknown.
270 * @param radioSourcePositionCovariance covariance of radio source position or null if
271 * unknown.
272 * @return variance of RSSI measured at non located fingerprint reading.
273 */
274 @Override
275 @SuppressWarnings("Duplicates")
276 protected Double propagateVariances(
277 final double fingerprintRssi, final double pathlossExponent,
278 final Point2D fingerprintPosition, final Point2D radioSourcePosition,
279 final Point2D estimatedPosition, final Double fingerprintRssiVariance,
280 final Double pathlossExponentVariance,
281 final Matrix fingerprintPositionCovariance,
282 final Matrix radioSourcePositionCovariance) {
283 try {
284 final var dist = Utils.propagateVariancesToRssiVarianceSecondOrderNonLinear2D(fingerprintRssi,
285 pathlossExponent, fingerprintPosition, radioSourcePosition, estimatedPosition,
286 fingerprintRssiVariance, pathlossExponentVariance, fingerprintPositionCovariance,
287 radioSourcePositionCovariance, null);
288
289 if (dist == null) {
290 return null;
291 }
292
293 final var covariance = dist.getCovariance();
294 if (covariance == null) {
295 return null;
296 }
297
298 return covariance.getElementAt(0, 0);
299
300 } catch (IndoorException e) {
301 return null;
302 }
303 }
304 }