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.InhomogeneousPoint2D;
19 import com.irurueta.geometry.Point2D;
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.NonLinearLeastSquaresLateration2DSolver;
26
27 import java.util.List;
28
29 /**
30 * Estimates 2D 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 NonLinearRangingAndRssiPositionEstimator2D extends NonLinearRangingAndRssiPositionEstimator<Point2D> {
37
38 /**
39 * Constructor.
40 */
41 public NonLinearRangingAndRssiPositionEstimator2D() {
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 NonLinearRangingAndRssiPositionEstimator2D(final List<? extends RadioSourceLocated<Point2D>> 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 NonLinearRangingAndRssiPositionEstimator2D(
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 NonLinearRangingAndRssiPositionEstimator2D(
86 final List<? extends RadioSourceLocated<Point2D>> 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 NonLinearRangingAndRssiPositionEstimator2D(final RangingAndRssiPositionEstimatorListener<Point2D> 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 NonLinearRangingAndRssiPositionEstimator2D(
115 final List<? extends RadioSourceLocated<Point2D>> sources,
116 final RangingAndRssiPositionEstimatorListener<Point2D> 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 NonLinearRangingAndRssiPositionEstimator2D(
131 final Fingerprint<? extends RadioSource, ? extends RangingAndRssiReading<?
132 extends RadioSource>> fingerprint,
133 final RangingAndRssiPositionEstimatorListener<Point2D> 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 NonLinearRangingAndRssiPositionEstimator2D(
151 final List<? extends RadioSourceLocated<Point2D>> sources,
152 final Fingerprint<? extends RadioSource, ? extends RangingAndRssiReading<?
153 extends RadioSource>> fingerprint,
154 final RangingAndRssiPositionEstimatorListener<Point2D> 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 NonLinearRangingAndRssiPositionEstimator2D(final Point2D 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 NonLinearRangingAndRssiPositionEstimator2D(
181 final List<? extends RadioSourceLocated<Point2D>> sources, final Point2D initialPosition) {
182 super(initialPosition);
183 initialize();
184 internalSetSources(sources);
185 }
186
187 /**
188 * Constructor.
189 *
190 * @param fingerprint fingerprint containing ranging readings at an unknown
191 * location for provided located radio sources.
192 * @param initialPosition initial position to start position estimation.
193 * @throws IllegalArgumentException if provided fingerprint is null.
194 */
195 public NonLinearRangingAndRssiPositionEstimator2D(
196 final Fingerprint<? extends RadioSource, ? extends RangingAndRssiReading<?
197 extends RadioSource>> fingerprint,
198 final Point2D initialPosition) {
199 super(initialPosition);
200 initialize();
201 internalSetFingerprint(fingerprint);
202 }
203
204 /**
205 * Constructor.
206 *
207 * @param sources located radio sources used for lateration.
208 * @param fingerprint fingerprint containing readings at an unknown location
209 * for provided located radio sources.
210 * @param initialPosition initial position to start position estimation
211 * @throws IllegalArgumentException if either provided sources or fingerprint is
212 * null or the number of provided sources is less
213 * than the required minimum.
214 */
215 public NonLinearRangingAndRssiPositionEstimator2D(
216 final List<? extends RadioSourceLocated<Point2D>> sources,
217 final Fingerprint<? extends RadioSource, ? extends RangingAndRssiReading<?
218 extends RadioSource>> fingerprint,
219 final Point2D initialPosition) {
220 super(initialPosition);
221 initialize();
222 internalSetSources(sources);
223 internalSetFingerprint(fingerprint);
224 }
225
226 /**
227 * Constructor.
228 *
229 * @param initialPosition initial position to start position estimation.
230 * @param listener listener in charge of handling events.
231 */
232 public NonLinearRangingAndRssiPositionEstimator2D(
233 final Point2D initialPosition, final RangingAndRssiPositionEstimatorListener<Point2D> listener) {
234 super(initialPosition, listener);
235 initialize();
236 }
237
238 /**
239 * Constructor.
240 *
241 * @param sources located radio sources used for lateration.
242 * @param initialPosition initial position to start position estimation.
243 * @param listener listener in charge of handling events.
244 * @throws IllegalArgumentException if provided sources is null or the number of
245 * provided sources is less than the required
246 * minimum.
247 */
248 public NonLinearRangingAndRssiPositionEstimator2D(
249 final List<? extends RadioSourceLocated<Point2D>> sources, final Point2D initialPosition,
250 final RangingAndRssiPositionEstimatorListener<Point2D> listener) {
251 super(initialPosition, listener);
252 initialize();
253 internalSetSources(sources);
254 }
255
256 /**
257 * Constructor.
258 *
259 * @param fingerprint fingerprint containing readings at an unknown location
260 * for provided located radio sources.
261 * @param initialPosition initial position to start position estimation.
262 * @param listener listener in charge of handling events.
263 * @throws IllegalArgumentException if provided fingerprint is null.
264 */
265 public NonLinearRangingAndRssiPositionEstimator2D(
266 final Fingerprint<? extends RadioSource, ? extends RangingAndRssiReading<?
267 extends RadioSource>> fingerprint,
268 final Point2D initialPosition,
269 final RangingAndRssiPositionEstimatorListener<Point2D> listener) {
270 super(initialPosition, listener);
271 initialize();
272 internalSetFingerprint(fingerprint);
273 }
274
275 /**
276 * Constructor.
277 *
278 * @param sources located radio sources used for lateration.
279 * @param fingerprint fingerprint containing readings at an unknown location
280 * for provided located radio sources.
281 * @param initialPosition initial position to start position estimation.
282 * @param listener listener in charge of handling events.
283 * @throws IllegalArgumentException if either provided sources or fingerprint is
284 * null or the number of provided sources is less
285 * than the required minimum.
286 */
287 public NonLinearRangingAndRssiPositionEstimator2D(
288 final List<? extends RadioSourceLocated<Point2D>> sources,
289 final Fingerprint<? extends RadioSource, ? extends RangingAndRssiReading<?
290 extends RadioSource>> fingerprint,
291 final Point2D initialPosition,
292 final RangingAndRssiPositionEstimatorListener<Point2D> listener) {
293 super(initialPosition, listener);
294 initialize();
295 internalSetSources(sources);
296 internalSetFingerprint(fingerprint);
297 }
298
299 /**
300 * Gets estimated position.
301 *
302 * @return estimated position.
303 */
304 public Point2D getEstimatedPosition() {
305 if (estimatedPositionCoordinates == null) {
306 return null;
307 }
308
309 final var result = new InhomogeneousPoint2D();
310 getEstimatedPosition(result);
311 return result;
312 }
313
314 /**
315 * Sets positions, distances and standard deviations of distances on internal
316 * lateration solver.
317 *
318 * @param positions positions to be set.
319 * @param distances distances to be set.
320 * @param distanceStandardDeviations standard deviations of distances to be set.
321 */
322 @Override
323 @SuppressWarnings("Duplicates")
324 protected void setPositionsDistancesAndDistanceStandardDeviations(
325 final List<Point2D> positions, final List<Double> distances,
326 final List<Double> distanceStandardDeviations) {
327
328 final var size = positions.size();
329 Point2D[] positionsArray = new InhomogeneousPoint2D[size];
330 positionsArray = positions.toArray(positionsArray);
331
332 final var distancesArray = new double[size];
333 final var distanceStandardDeviationsArray = new double[size];
334 for (var i = 0; i < size; i++) {
335 distancesArray[i] = distances.get(i);
336 distanceStandardDeviationsArray[i] = distanceStandardDeviations.get(i);
337 }
338
339 try {
340 trilaterationSolver.setPositionsDistancesAndStandardDeviations(positionsArray, distancesArray,
341 distanceStandardDeviationsArray);
342 } catch (final LockedException e) {
343 throw new IllegalArgumentException(e);
344 }
345 }
346
347 /**
348 * Initializes lateration solver.
349 */
350 private void initialize() {
351 trilaterationSolver = new NonLinearLeastSquaresLateration2DSolver(laterationSolverListener);
352 }
353 }