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.position;
17
18 import com.irurueta.geometry.InhomogeneousPoint3D;
19 import com.irurueta.geometry.Point3D;
20 import com.irurueta.navigation.LockedException;
21 import com.irurueta.navigation.indoor.Fingerprint;
22 import com.irurueta.navigation.indoor.RadioSource;
23 import com.irurueta.navigation.indoor.RadioSourceLocated;
24 import com.irurueta.navigation.indoor.RangingAndRssiReading;
25 import com.irurueta.navigation.lateration.NonLinearLeastSquaresLateration3DSolver;
26
27 import java.util.List;
28
29 /**
30 * Estimates 3D position non-linearly using located radio sources and their ranging+RSSI
31 * readings at unknown locations.
32 * These kind of estimators can be used to determine the position of a given device by
33 * getting ranging+RSSI readings at an unknown location of different radio sources whose
34 * locations are known.
35 */
36 public class NonLinearRangingAndRssiPositionEstimator3D extends NonLinearRangingAndRssiPositionEstimator<Point3D> {
37
38 /**
39 * Constructor.
40 */
41 public NonLinearRangingAndRssiPositionEstimator3D() {
42 super();
43 initialize();
44 }
45
46 /**
47 * Constructor
48 *
49 * @param sources located radio sources used for lateration.
50 * @throws IllegalArgumentException if provided sources is null or the number of
51 * provided sources is less than the required
52 * minimum.
53 */
54 public NonLinearRangingAndRssiPositionEstimator3D(final List<? extends RadioSourceLocated<Point3D>> sources) {
55 super();
56 initialize();
57 internalSetSources(sources);
58 }
59
60 /**
61 * Constructor.
62 *
63 * @param fingerprint fingerprint containing RSSI readings at an unknown location
64 * for provided located radio sources.
65 * @throws IllegalArgumentException if provided fingerprint is null.
66 */
67 public NonLinearRangingAndRssiPositionEstimator3D(
68 final Fingerprint<? extends RadioSource, ? extends RangingAndRssiReading<?
69 extends RadioSource>> fingerprint) {
70 super();
71 initialize();
72 internalSetFingerprint(fingerprint);
73 }
74
75 /**
76 * Constructor.
77 *
78 * @param sources located radio sources used for lateration.
79 * @param fingerprint fingerprint containing readings at an unknown location for
80 * provided located radio sources.
81 * @throws IllegalArgumentException if either provided sources or fingerprint is
82 * null or the number of provided sources is less
83 * than the required minimum.
84 */
85 public NonLinearRangingAndRssiPositionEstimator3D(
86 final List<? extends RadioSourceLocated<Point3D>> sources,
87 final Fingerprint<? extends RadioSource, ? extends RangingAndRssiReading<?
88 extends RadioSource>> fingerprint) {
89 super();
90 initialize();
91 internalSetSources(sources);
92 internalSetFingerprint(fingerprint);
93 }
94
95 /**
96 * Constructor.
97 *
98 * @param listener listener in charge of handling events.
99 */
100 public NonLinearRangingAndRssiPositionEstimator3D(final RangingAndRssiPositionEstimatorListener<Point3D> listener) {
101 super(listener);
102 initialize();
103 }
104
105 /**
106 * Constructor.
107 *
108 * @param sources located radio sources used for lateration.
109 * @param listener listener in charge of handling events.
110 * @throws IllegalArgumentException if provided sources is null or the number of
111 * provided sources is less than the required
112 * minimum.
113 */
114 public NonLinearRangingAndRssiPositionEstimator3D(
115 final List<? extends RadioSourceLocated<Point3D>> sources,
116 final RangingAndRssiPositionEstimatorListener<Point3D> listener) {
117 super(listener);
118 initialize();
119 internalSetSources(sources);
120 }
121
122 /**
123 * Constructor.
124 *
125 * @param fingerprint fingerprint containing readings at an unknown location for
126 * provided located radio sources.
127 * @param listener listener in charge of handling events.
128 * @throws IllegalArgumentException if provided fingerprint is null.
129 */
130 public NonLinearRangingAndRssiPositionEstimator3D(
131 final Fingerprint<? extends RadioSource, ? extends RangingAndRssiReading<?
132 extends RadioSource>> fingerprint,
133 final RangingAndRssiPositionEstimatorListener<Point3D> listener) {
134 super(listener);
135 initialize();
136 internalSetFingerprint(fingerprint);
137 }
138
139 /**
140 * Constructor.
141 *
142 * @param sources located radio sources used for lateration.
143 * @param fingerprint fingerprint containing readings at an unknown location for
144 * provided located radio sources.
145 * @param listener listener in charge of handling events.
146 * @throws IllegalArgumentException if either provided sources or fingerprint is
147 * null or the number of provided sources is less
148 * than the required minimum.
149 */
150 public NonLinearRangingAndRssiPositionEstimator3D(
151 final List<? extends RadioSourceLocated<Point3D>> sources,
152 final Fingerprint<? extends RadioSource, ? extends RangingAndRssiReading<?
153 extends RadioSource>> fingerprint,
154 final RangingAndRssiPositionEstimatorListener<Point3D> listener) {
155 super(listener);
156 initialize();
157 internalSetSources(sources);
158 internalSetFingerprint(fingerprint);
159 }
160
161 /**
162 * Constructor.
163 *
164 * @param initialPosition initial position to start position estimation.
165 */
166 public NonLinearRangingAndRssiPositionEstimator3D(final Point3D initialPosition) {
167 super(initialPosition);
168 initialize();
169 }
170
171 /**
172 * Constructor.
173 *
174 * @param sources located radio sources used for lateration.
175 * @param initialPosition initial position to start position estimation.
176 * @throws IllegalArgumentException if provided sources is null or the number of
177 * provided sources is less than the required
178 * minimum.
179 */
180 public NonLinearRangingAndRssiPositionEstimator3D(
181 final List<? extends RadioSourceLocated<Point3D>> sources,
182 final Point3D initialPosition) {
183 super(initialPosition);
184 initialize();
185 internalSetSources(sources);
186 }
187
188 /**
189 * Constructor.
190 *
191 * @param fingerprint fingerprint containing ranging readings at an unknown
192 * location for provided located radio sources.
193 * @param initialPosition initial position to start position estimation.
194 * @throws IllegalArgumentException if provided fingerprint is null.
195 */
196 public NonLinearRangingAndRssiPositionEstimator3D(
197 final Fingerprint<? extends RadioSource, ? extends RangingAndRssiReading<?
198 extends RadioSource>> fingerprint,
199 final Point3D initialPosition) {
200 super(initialPosition);
201 initialize();
202 internalSetFingerprint(fingerprint);
203 }
204
205 /**
206 * Constructor.
207 *
208 * @param sources located radio sources used for lateration.
209 * @param fingerprint fingerprint containing readings at an unknown location
210 * for provided located radio sources.
211 * @param initialPosition initial position to start position estimation
212 * @throws IllegalArgumentException if either provided sources or fingerprint is
213 * null or the number of provided sources is less
214 * than the required minimum.
215 */
216 public NonLinearRangingAndRssiPositionEstimator3D(
217 final List<? extends RadioSourceLocated<Point3D>> sources,
218 final Fingerprint<? extends RadioSource, ? extends RangingAndRssiReading<?
219 extends RadioSource>> fingerprint,
220 final Point3D initialPosition) {
221 super(initialPosition);
222 initialize();
223 internalSetSources(sources);
224 internalSetFingerprint(fingerprint);
225 }
226
227 /**
228 * Constructor.
229 *
230 * @param initialPosition initial position to start position estimation.
231 * @param listener listener in charge of handling events.
232 */
233 public NonLinearRangingAndRssiPositionEstimator3D(
234 final Point3D initialPosition, final RangingAndRssiPositionEstimatorListener<Point3D> listener) {
235 super(initialPosition, listener);
236 initialize();
237 }
238
239 /**
240 * Constructor.
241 *
242 * @param sources located radio sources used for lateration.
243 * @param initialPosition initial position to start position estimation.
244 * @param listener listener in charge of handling events.
245 * @throws IllegalArgumentException if provided sources is null or the number of
246 * provided sources is less than the required
247 * minimum.
248 */
249 public NonLinearRangingAndRssiPositionEstimator3D(
250 final List<? extends RadioSourceLocated<Point3D>> sources,
251 final Point3D initialPosition,
252 final RangingAndRssiPositionEstimatorListener<Point3D> listener) {
253 super(initialPosition, listener);
254 initialize();
255 internalSetSources(sources);
256 }
257
258 /**
259 * Constructor.
260 *
261 * @param fingerprint fingerprint containing readings at an unknown location
262 * for provided located radio sources.
263 * @param initialPosition initial position to start position estimation.
264 * @param listener listener in charge of handling events.
265 * @throws IllegalArgumentException if provided fingerprint is null.
266 */
267 public NonLinearRangingAndRssiPositionEstimator3D(
268 final Fingerprint<? extends RadioSource, ? extends RangingAndRssiReading<?
269 extends RadioSource>> fingerprint,
270 final Point3D initialPosition,
271 final RangingAndRssiPositionEstimatorListener<Point3D> listener) {
272 super(initialPosition, listener);
273 initialize();
274 internalSetFingerprint(fingerprint);
275 }
276
277 /**
278 * Constructor.
279 *
280 * @param sources located radio sources used for lateration.
281 * @param fingerprint fingerprint containing readings at an unknown location
282 * for provided located radio sources.
283 * @param initialPosition initial position to start position estimation.
284 * @param listener listener in charge of handling events.
285 * @throws IllegalArgumentException if either provided sources or fingerprint is
286 * null or the number of provided sources is less
287 * than the required minimum.
288 */
289 public NonLinearRangingAndRssiPositionEstimator3D(
290 final List<? extends RadioSourceLocated<Point3D>> sources,
291 final Fingerprint<? extends RadioSource, ? extends RangingAndRssiReading<?
292 extends RadioSource>> fingerprint,
293 final Point3D initialPosition,
294 final RangingAndRssiPositionEstimatorListener<Point3D> listener) {
295 super(initialPosition, listener);
296 initialize();
297 internalSetSources(sources);
298 internalSetFingerprint(fingerprint);
299 }
300
301 /**
302 * Gets estimated position.
303 *
304 * @return estimated position.
305 */
306 public Point3D getEstimatedPosition() {
307 if (estimatedPositionCoordinates == null) {
308 return null;
309 }
310
311 final var result = new InhomogeneousPoint3D();
312 getEstimatedPosition(result);
313 return result;
314 }
315
316 /**
317 * Sets positions, distances and standard deviations of distances on internal
318 * lateration solver.
319 *
320 * @param positions positions to be set.
321 * @param distances distances to be set.
322 * @param distanceStandardDeviations standard deviations of distances to be set.
323 */
324 @Override
325 @SuppressWarnings("Duplicates")
326 protected void setPositionsDistancesAndDistanceStandardDeviations(
327 final List<Point3D> positions, final List<Double> distances,
328 final List<Double> distanceStandardDeviations) {
329
330 final var size = positions.size();
331 Point3D[] positionsArray = new InhomogeneousPoint3D[size];
332 positionsArray = positions.toArray(positionsArray);
333
334 final var distancesArray = new double[size];
335 final var distanceStandardDeviationsArray = new double[size];
336 for (var i = 0; i < size; i++) {
337 distancesArray[i] = distances.get(i);
338 distanceStandardDeviationsArray[i] = distanceStandardDeviations.get(i);
339 }
340
341 try {
342 trilaterationSolver.setPositionsDistancesAndStandardDeviations(positionsArray, distancesArray,
343 distanceStandardDeviationsArray);
344 } catch (final LockedException e) {
345 throw new IllegalArgumentException(e);
346 }
347 }
348
349 /**
350 * Initializes lateration solver.
351 */
352 private void initialize() {
353 trilaterationSolver = new NonLinearLeastSquaresLateration3DSolver(laterationSolverListener);
354 }
355 }