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 first-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 FirstOrderNonLinearFingerprintPositionEstimator2D extends NonLinearFingerprintPositionEstimator2D {
42
43 /**
44 * Constructor.
45 */
46 public FirstOrderNonLinearFingerprintPositionEstimator2D() {
47 }
48
49 /**
50 * Constructor.
51 *
52 * @param listener listener in charge of handling events.
53 */
54 public FirstOrderNonLinearFingerprintPositionEstimator2D(
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 FirstOrderNonLinearFingerprintPositionEstimator2D(
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 FirstOrderNonLinearFingerprintPositionEstimator2D(
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 FirstOrderNonLinearFingerprintPositionEstimator2D(
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 FirstOrderNonLinearFingerprintPositionEstimator2D(
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.FIRST_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
191 final var xi = params[0];
192 final var yi = params[1];
193
194 // received power
195 final var pr = point[0];
196
197 // fingerprint coordinates
198 final var x1 = point[1];
199 final var y1 = point[2];
200
201 // radio source coordinates
202 final var xa = point[3];
203 final var ya = point[4];
204
205 // path loss exponent
206 final var n = point[5];
207
208 final var ln10 = Math.log(10.0);
209
210 final var diffXi1 = xi - x1;
211 final var diffYi1 = yi - y1;
212
213 final var diffX1a = x1 - xa;
214 final var diffY1a = y1 - ya;
215
216 final var diffX1a2 = diffX1a * diffX1a;
217 final var diffY1a2 = diffY1a * diffY1a;
218
219 final var d1a2 = diffX1a2 + diffY1a2;
220
221 final var value1 = -10.0 * n * diffX1a / (ln10 * d1a2);
222 final var value2 = -10.0 * n * diffY1a / (ln10 * d1a2);
223
224 final var result = pr + value1 * diffXi1 + value2 * diffYi1;
225
226 // derivative respect xi
227 // diff(Pr(pi))/diff(xi) = - 10*n*(x1 - xa)/(ln(10)*d1a^2)
228 derivatives[0] = value1;
229
230 // derivative respect yi
231 // diff(Pr(pi))/diff(yi) = - 10*n*(y1 - ya)/(ln(10)*d1a^2)
232 derivatives[1] = value2;
233
234 return result;
235 }
236
237 /**
238 * Propagates provided variances into RSSI variance of non-located fingerprint
239 * reading.
240 *
241 * @param fingerprintRssi closest located fingerprint reading RSSI expressed in dBm's.
242 * @param pathlossExponent path-loss exponent.
243 * @param fingerprintPosition position of closest fingerprint.
244 * @param radioSourcePosition radio source position associated to fingerprint reading.
245 * @param estimatedPosition position to be estimated. Usually this is equal to the
246 * initial position used by a non-linear algorithm.
247 * @param fingerprintRssiVariance variance of fingerprint RSSI or null if unknown.
248 * @param pathlossExponentVariance variance of path-loss exponent or null if unknown.
249 * @param fingerprintPositionCovariance covariance of fingerprint position or null if
250 * unknown.
251 * @param radioSourcePositionCovariance covariance of radio source position or null if
252 * unknown.
253 * @return variance of RSSI measured at non located fingerprint reading.
254 */
255 @Override
256 @SuppressWarnings("Duplicates")
257 protected Double propagateVariances(
258 final double fingerprintRssi, final double pathlossExponent, final Point2D fingerprintPosition,
259 final Point2D radioSourcePosition, final Point2D estimatedPosition, final Double fingerprintRssiVariance,
260 final Double pathlossExponentVariance, final Matrix fingerprintPositionCovariance,
261 final Matrix radioSourcePositionCovariance) {
262 try {
263 final var dist = Utils.propagateVariancesToRssiVarianceFirstOrderNonLinear2D(fingerprintRssi,
264 pathlossExponent, fingerprintPosition, radioSourcePosition, estimatedPosition,
265 fingerprintRssiVariance, pathlossExponentVariance, fingerprintPositionCovariance,
266 radioSourcePositionCovariance, null);
267 if (dist == null) {
268 return null;
269 }
270
271 final var covariance = dist.getCovariance();
272 if (covariance == null) {
273 return null;
274 }
275
276 return covariance.getElementAt(0, 0);
277
278 } catch (final IndoorException e) {
279 return null;
280 }
281 }
282 }