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.Reading;
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 readings
31 * at unknown locations.
32 * These kind of estimators can be used to determine the position of a given device by
33 * getting readings at an unknown location of different radio sources whose locations
34 * are known.
35 */
36 public class NonLinearMixedPositionEstimator2D extends NonLinearMixedPositionEstimator<Point2D> {
37
38 /**
39 * Constructor.
40 */
41 public NonLinearMixedPositionEstimator2D() {
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 NonLinearMixedPositionEstimator2D(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 NonLinearMixedPositionEstimator2D(
68 final Fingerprint<? extends RadioSource, ? extends Reading<? 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 NonLinearMixedPositionEstimator2D(
85 final List<? extends RadioSourceLocated<Point2D>> sources,
86 final Fingerprint<? extends RadioSource, ? extends Reading<? 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 NonLinearMixedPositionEstimator2D(final MixedPositionEstimatorListener<Point2D> 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 NonLinearMixedPositionEstimator2D(
113 final List<? extends RadioSourceLocated<Point2D>> sources,
114 final MixedPositionEstimatorListener<Point2D> 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 NonLinearMixedPositionEstimator2D(
129 final Fingerprint<? extends RadioSource, ? extends Reading<? extends RadioSource>> fingerprint,
130 final MixedPositionEstimatorListener<Point2D> 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 NonLinearMixedPositionEstimator2D(
148 final List<? extends RadioSourceLocated<Point2D>> sources,
149 final Fingerprint<? extends RadioSource, ? extends Reading<? extends RadioSource>> fingerprint,
150 final MixedPositionEstimatorListener<Point2D> 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 NonLinearMixedPositionEstimator2D(final Point2D 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 NonLinearMixedPositionEstimator2D(
177 final List<? extends RadioSourceLocated<Point2D>> sources,
178 final Point2D initialPosition) {
179 super(initialPosition);
180 initialize();
181 internalSetSources(sources);
182 }
183
184 /**
185 * Constructor.
186 *
187 * @param fingerprint fingerprint containing ranging readings at an unknown
188 * location for provided located radio sources.
189 * @param initialPosition initial position to start position estimation.
190 * @throws IllegalArgumentException if provided fingerprint is null.
191 */
192 public NonLinearMixedPositionEstimator2D(
193 final Fingerprint<? extends RadioSource, ? extends Reading<? extends RadioSource>> fingerprint,
194 final Point2D initialPosition) {
195 super(initialPosition);
196 initialize();
197 internalSetFingerprint(fingerprint);
198 }
199
200 /**
201 * Constructor.
202 *
203 * @param sources located radio sources used for lateration.
204 * @param fingerprint fingerprint containing readings at an unknown location
205 * for provided located radio sources.
206 * @param initialPosition initial position to start position estimation
207 * @throws IllegalArgumentException if either provided sources or fingerprint is
208 * null or the number of provided sources is less
209 * than the required minimum.
210 */
211 public NonLinearMixedPositionEstimator2D(
212 final List<? extends RadioSourceLocated<Point2D>> sources,
213 final Fingerprint<? extends RadioSource, ? extends Reading<? extends RadioSource>> fingerprint,
214 final Point2D initialPosition) {
215 super(initialPosition);
216 initialize();
217 internalSetSources(sources);
218 internalSetFingerprint(fingerprint);
219 }
220
221 /**
222 * Constructor.
223 *
224 * @param initialPosition initial position to start position estimation.
225 * @param listener listener in charge of handling events.
226 */
227 public NonLinearMixedPositionEstimator2D(
228 final Point2D initialPosition, final MixedPositionEstimatorListener<Point2D> listener) {
229 super(initialPosition, listener);
230 initialize();
231 }
232
233 /**
234 * Constructor.
235 *
236 * @param sources located radio sources used for lateration.
237 * @param initialPosition initial position to start position estimation.
238 * @param listener listener in charge of handling events.
239 * @throws IllegalArgumentException if provided sources is null or the number of
240 * provided sources is less than the required
241 * minimum.
242 */
243 public NonLinearMixedPositionEstimator2D(
244 final List<? extends RadioSourceLocated<Point2D>> sources, final Point2D initialPosition,
245 final MixedPositionEstimatorListener<Point2D> listener) {
246 super(initialPosition, listener);
247 initialize();
248 internalSetSources(sources);
249 }
250
251 /**
252 * Constructor.
253 *
254 * @param fingerprint fingerprint containing readings at an unknown location
255 * for provided located radio sources.
256 * @param initialPosition initial position to start position estimation.
257 * @param listener listener in charge of handling events.
258 * @throws IllegalArgumentException if provided fingerprint is null.
259 */
260 public NonLinearMixedPositionEstimator2D(
261 final Fingerprint<? extends RadioSource, ? extends Reading<? extends RadioSource>> fingerprint,
262 final Point2D initialPosition, final MixedPositionEstimatorListener<Point2D> listener) {
263 super(initialPosition, listener);
264 initialize();
265 internalSetFingerprint(fingerprint);
266 }
267
268 /**
269 * Constructor.
270 *
271 * @param sources located radio sources used for lateration.
272 * @param fingerprint fingerprint containing readings at an unknown location
273 * for provided located radio sources.
274 * @param initialPosition initial position to start position estimation.
275 * @param listener listener in charge of handling events.
276 * @throws IllegalArgumentException if either provided sources or fingerprint is
277 * null or the number of provided sources is less
278 * than the required minimum.
279 */
280 public NonLinearMixedPositionEstimator2D(
281 final List<? extends RadioSourceLocated<Point2D>> sources,
282 final Fingerprint<? extends RadioSource, ? extends Reading<? extends RadioSource>> fingerprint,
283 final Point2D initialPosition, final MixedPositionEstimatorListener<Point2D> listener) {
284 super(initialPosition, listener);
285 initialize();
286 internalSetSources(sources);
287 internalSetFingerprint(fingerprint);
288 }
289
290 /**
291 * Gets estimated position.
292 *
293 * @return estimated position.
294 */
295 public Point2D getEstimatedPosition() {
296 if (estimatedPositionCoordinates == null) {
297 return null;
298 }
299
300 final var result = new InhomogeneousPoint2D();
301 getEstimatedPosition(result);
302 return result;
303 }
304
305 /**
306 * Sets positions, distances and standard deviations of distances on internal
307 * lateration solver.
308 *
309 * @param positions positions to be set.
310 * @param distances distances to be set.
311 * @param distanceStandardDeviations standard deviations of distances to be set.
312 */
313 @Override
314 @SuppressWarnings("Duplicates")
315 protected void setPositionsDistancesAndDistanceStandardDeviations(
316 final List<Point2D> positions, final List<Double> distances,
317 final List<Double> distanceStandardDeviations) {
318
319 final var size = positions.size();
320 Point2D[] positionsArray = new InhomogeneousPoint2D[size];
321 positionsArray = positions.toArray(positionsArray);
322
323 final var distancesArray = new double[size];
324 final var distanceStandardDeviationsArray = new double[size];
325 for (var i = 0; i < size; i++) {
326 distancesArray[i] = distances.get(i);
327 distanceStandardDeviationsArray[i] = distanceStandardDeviations.get(i);
328 }
329
330 try {
331 trilaterationSolver.setPositionsDistancesAndStandardDeviations(positionsArray, distancesArray,
332 distanceStandardDeviationsArray);
333 } catch (final LockedException e) {
334 throw new IllegalArgumentException(e);
335 }
336 }
337
338 /**
339 * Initializes lateration solver.
340 */
341 private void initialize() {
342 trilaterationSolver = new NonLinearLeastSquaresLateration2DSolver(laterationSolverListener);
343 }
344 }