1 /*
2 * Copyright (C) 2019 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.InhomogeneousPoint3D;
20 import com.irurueta.geometry.Point3D;
21 import com.irurueta.navigation.indoor.IndoorException;
22 import com.irurueta.navigation.indoor.RadioSource;
23 import com.irurueta.navigation.indoor.RadioSourceLocated;
24 import com.irurueta.navigation.indoor.RssiFingerprint;
25 import com.irurueta.navigation.indoor.RssiFingerprintLocated;
26 import com.irurueta.navigation.indoor.RssiReading;
27 import com.irurueta.navigation.indoor.Utils;
28
29 import java.util.List;
30
31 /**
32 * Position and radio source estimator in 3D based only on located fingerprints
33 * containing RSSI readings.
34 * This class estimates 3D position of a new fingerprint and the 3D position of all
35 * radio sources associated to fingerprints whose location is known.
36 */
37 public class NonLinearFingerprintPositionAndRadioSourceEstimator3D extends
38 NonLinearFingerprintPositionAndRadioSourceEstimator<Point3D> {
39
40 /**
41 * Constructor.
42 */
43 public NonLinearFingerprintPositionAndRadioSourceEstimator3D() {
44 }
45
46 /**
47 * Constructor.
48 *
49 * @param listener listener in charge of handling events.
50 */
51 public NonLinearFingerprintPositionAndRadioSourceEstimator3D(
52 final FingerprintPositionAndRadioSourceEstimatorListener<Point3D> listener) {
53 super(listener);
54 }
55
56 /**
57 * Constructor.
58 *
59 * @param locatedFingerprints located fingerprints containing RSSI readings.
60 * @param fingerprint fingerprint containing readings at an unknown location
61 * for provided located fingerprints.
62 * @throws IllegalArgumentException if either non located fingerprint or located
63 * fingerprints are null.
64 */
65 public NonLinearFingerprintPositionAndRadioSourceEstimator3D(
66 final List<? extends RssiFingerprintLocated<? extends RadioSource,
67 ? extends RssiReading<? extends RadioSource>, Point3D>> locatedFingerprints,
68 final RssiFingerprint<? extends RadioSource,
69 ? extends RssiReading<? extends RadioSource>> fingerprint) {
70 super(locatedFingerprints, fingerprint);
71 }
72
73 /**
74 * Constructor.
75 *
76 * @param locatedFingerprints located fingerprints containing RSSI readings.
77 * @param fingerprint fingerprint containing readings at an unknown location
78 * for provided located fingerprints.
79 * @param listener listener in charge of handling events.
80 * @throws IllegalArgumentException if either non located fingerprint or located
81 * fingerprints are null.
82 */
83 public NonLinearFingerprintPositionAndRadioSourceEstimator3D(
84 final List<? extends RssiFingerprintLocated<? extends RadioSource,
85 ? extends RssiReading<? extends RadioSource>, Point3D>> locatedFingerprints,
86 final RssiFingerprint<? extends RadioSource,
87 ? extends RssiReading<? extends RadioSource>> fingerprint,
88 final FingerprintPositionAndRadioSourceEstimatorListener<Point3D> listener) {
89 super(locatedFingerprints, fingerprint, listener);
90 }
91
92 /**
93 * Constructor.
94 *
95 * @param locatedFingerprints located fingerprints containing RSSI readings.
96 * @param fingerprint fingerprint containing readings at an unknown location
97 * for provided located fingerprints.
98 * @param initialPosition initial position to be assumed on non located fingerprint or
99 * null if unknown.
100 * @throws IllegalArgumentException if either non located fingerprint or located
101 * fingerprints are null.
102 */
103 public NonLinearFingerprintPositionAndRadioSourceEstimator3D(
104 final List<? extends RssiFingerprintLocated<? extends RadioSource,
105 ? extends RssiReading<? extends RadioSource>, Point3D>> locatedFingerprints,
106 final RssiFingerprint<? extends RadioSource,
107 ? extends RssiReading<? extends RadioSource>> fingerprint,
108 final Point3D initialPosition) {
109 super(locatedFingerprints, fingerprint, initialPosition);
110 }
111
112 /**
113 * Constructor.
114 *
115 * @param locatedFingerprints located fingerprints containing RSSI readings.
116 * @param fingerprint fingerprint containing readings at an unknown location
117 * for provided located fingerprints.
118 * @param initialPosition initial position to be assumed on non located fingerprint or
119 * null if unknown.
120 * @param listener listener in charge of handling events.
121 * @throws IllegalArgumentException if either non located fingerprint or located
122 * fingerprints are null.
123 */
124 public NonLinearFingerprintPositionAndRadioSourceEstimator3D(
125 final List<? extends RssiFingerprintLocated<? extends RadioSource,
126 ? extends RssiReading<? extends RadioSource>, Point3D>> locatedFingerprints,
127 final RssiFingerprint<? extends RadioSource,
128 ? extends RssiReading<? extends RadioSource>> fingerprint,
129 final Point3D initialPosition,
130 final FingerprintPositionAndRadioSourceEstimatorListener<Point3D> listener) {
131 super(locatedFingerprints, fingerprint, initialPosition, listener);
132 }
133
134 /**
135 * Constructor.
136 *
137 * @param locatedFingerprints located fingerprints containing RSSI readings.
138 * @param fingerprint fingerprint containing readings at an unknown location
139 * for provided located fingerprints.
140 * @param initialLocatedSources sources containing initial location to be refined or null
141 * if unknown.
142 * @throws IllegalArgumentException if either non located fingerprint or located
143 * fingerprints are null.
144 */
145 public NonLinearFingerprintPositionAndRadioSourceEstimator3D(
146 final List<? extends RssiFingerprintLocated<? extends RadioSource,
147 ? extends RssiReading<? extends RadioSource>, Point3D>> locatedFingerprints,
148 final RssiFingerprint<? extends RadioSource,
149 ? extends RssiReading<? extends RadioSource>> fingerprint,
150 final List<? extends RadioSourceLocated<Point3D>> initialLocatedSources) {
151 super(locatedFingerprints, fingerprint, initialLocatedSources);
152 }
153
154 /**
155 * Constructor.
156 *
157 * @param locatedFingerprints located fingerprints containing RSSI readings.
158 * @param fingerprint fingerprint containing readings at an unknown location
159 * for provided located fingerprints.
160 * @param initialLocatedSources sources containing initial location to be refined or null
161 * if unknown.
162 * @param listener listener in charge of handling events.
163 * @throws IllegalArgumentException if either non located fingerprint or located
164 * fingerprints are null.
165 */
166 public NonLinearFingerprintPositionAndRadioSourceEstimator3D(
167 final List<? extends RssiFingerprintLocated<? extends RadioSource,
168 ? extends RssiReading<? extends RadioSource>, Point3D>> locatedFingerprints,
169 final RssiFingerprint<? extends RadioSource,
170 ? extends RssiReading<? extends RadioSource>> fingerprint,
171 final List<? extends RadioSourceLocated<Point3D>> initialLocatedSources,
172 final FingerprintPositionAndRadioSourceEstimatorListener<Point3D> listener) {
173 super(locatedFingerprints, fingerprint, initialLocatedSources, listener);
174 }
175
176 /**
177 * Constructor.
178 *
179 * @param locatedFingerprints located fingerprints containing RSSI readings.
180 * @param fingerprint fingerprint containing readings at an unknown location
181 * for provided located fingerprints.
182 * @param initialPosition initial position to be assumed on non located fingerprint or
183 * null if unknown.
184 * @param initialLocatedSources sources containing initial location to be refined or null
185 * if unknown.
186 * @throws IllegalArgumentException if either non located fingerprint or located
187 * fingerprints are null.
188 */
189 public NonLinearFingerprintPositionAndRadioSourceEstimator3D(
190 final List<? extends RssiFingerprintLocated<? extends RadioSource,
191 ? extends RssiReading<? extends RadioSource>, Point3D>> locatedFingerprints,
192 final RssiFingerprint<? extends RadioSource,
193 ? extends RssiReading<? extends RadioSource>> fingerprint,
194 final Point3D initialPosition,
195 final List<? extends RadioSourceLocated<Point3D>> initialLocatedSources) {
196 super(locatedFingerprints, fingerprint, initialPosition, initialLocatedSources);
197 }
198
199 /**
200 * Constructor.
201 *
202 * @param locatedFingerprints located fingerprints containing RSSI readings.
203 * @param fingerprint fingerprint containing readings at an unknown location
204 * for provided located fingerprints.
205 * @param initialPosition initial position to be assumed on non located fingerprint or
206 * null if unknown.
207 * @param initialLocatedSources sources containing initial location to be refined or null
208 * if unknown.
209 * @param listener listener in charge of handling events.
210 * @throws IllegalArgumentException if either non located fingerprint or located
211 * fingerprints are null.
212 */
213 public NonLinearFingerprintPositionAndRadioSourceEstimator3D(
214 final List<? extends RssiFingerprintLocated<? extends RadioSource,
215 ? extends RssiReading<? extends RadioSource>, Point3D>> locatedFingerprints,
216 final RssiFingerprint<? extends RadioSource,
217 ? extends RssiReading<? extends RadioSource>> fingerprint,
218 final Point3D initialPosition,
219 final List<? extends RadioSourceLocated<Point3D>> initialLocatedSources,
220 final FingerprintPositionAndRadioSourceEstimatorListener<Point3D> listener) {
221 super(locatedFingerprints, fingerprint, initialPosition, initialLocatedSources, listener);
222 }
223
224 /**
225 * Propagates provided variances into RSSI differences.
226 *
227 * @param pathlossExponent path-loss exponent.
228 * @param fingerprintPosition position of closest located fingerprint.
229 * @param radioSourcePosition radio source position associated to fingerprint reading.
230 * @param estimatedPosition position to be estimated. Usually this is equal to the
231 * initial position used by a non-linear algorithm.
232 * @param pathlossExponentVariance variance of path-loss exponent or null if unknown.
233 * @param fingerprintPositionCovariance covariance of fingerprint position or null if
234 * unknown.
235 * @param radioSourcePositionCovariance covariance of radio source position or null if
236 * unknown.
237 * @return variance of RSSI difference measured at non located fingerprint reading.
238 */
239 @Override
240 @SuppressWarnings("Duplicates")
241 protected Double propagateVariances(
242 final double pathlossExponent, final Point3D fingerprintPosition, final Point3D radioSourcePosition,
243 final Point3D estimatedPosition, final Double pathlossExponentVariance,
244 final Matrix fingerprintPositionCovariance, final Matrix radioSourcePositionCovariance) {
245 try {
246 final var dist = Utils.propagateVariancesToRssiDifferenceVariance3D(pathlossExponent, fingerprintPosition,
247 radioSourcePosition, estimatedPosition, pathlossExponentVariance, fingerprintPositionCovariance,
248 radioSourcePositionCovariance,
249 null);
250 if (dist == null) {
251 return null;
252 }
253
254 final var covariance = dist.getCovariance();
255 if (covariance == null) {
256 return null;
257 }
258
259 return covariance.getElementAt(0, 0);
260 } catch (final IndoorException e) {
261 return null;
262 }
263 }
264
265 /**
266 * Gets number of dimensions of points.
267 *
268 * @return number of dimensions of points.
269 */
270 @Override
271 public int getNumberOfDimensions() {
272 return Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH;
273 }
274
275 /**
276 * Create a point.
277 *
278 * @return point to be created.
279 */
280 @Override
281 protected Point3D createPoint() {
282 return new InhomogeneousPoint3D();
283 }
284 }