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