1 /*
2 * Copyright (C) 2018 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.lateration;
17
18 import com.irurueta.geometry.Circle;
19 import com.irurueta.geometry.InhomogeneousPoint2D;
20 import com.irurueta.geometry.Point2D;
21 import com.irurueta.navigation.LockedException;
22
23 /**
24 * Solves a Trilateration problem with an instance of the least squares optimizer.
25 */
26 public class NonLinearLeastSquaresLateration2DSolver extends NonLinearLeastSquaresLaterationSolver<Point2D> {
27
28 /**
29 * Constructor.
30 */
31 public NonLinearLeastSquaresLateration2DSolver() {
32 super();
33 }
34
35 /**
36 * Constructor.
37 *
38 * @param positions known positions of static nodes.
39 * @param distances euclidean distances from static nodes to mobile node.
40 * @throws IllegalArgumentException if either positions or distances are null, don't have the same length or their
41 * length is smaller than required (2 points).
42 */
43 public NonLinearLeastSquaresLateration2DSolver(final Point2D[] positions, final double[] distances) {
44 super(positions, distances);
45 }
46
47 /**
48 * Constructor.
49 *
50 * @param initialPosition initial position to start lateration solving.
51 */
52 public NonLinearLeastSquaresLateration2DSolver(final Point2D initialPosition) {
53 super(initialPosition);
54 }
55
56 /**
57 * Constructor.
58 *
59 * @param positions known positions of static nodes.
60 * @param distances euclidean distances from static nodes to mobile node.
61 * @param initialPosition initial position to start lateration solving.
62 * @throws IllegalArgumentException if either positions or distances are null, don't have the same length or their
63 * length is smaller than required (2 points).
64 */
65 public NonLinearLeastSquaresLateration2DSolver(
66 final Point2D[] positions, final double[] distances, final Point2D initialPosition) {
67 super(positions, distances, initialPosition);
68 }
69
70 /**
71 * Constructor.
72 *
73 * @param listener listener to be notified of events raised by this instance.
74 */
75 public NonLinearLeastSquaresLateration2DSolver(final LaterationSolverListener<Point2D> listener) {
76 super(listener);
77 }
78
79 /**
80 * Constructor.
81 *
82 * @param positions known positions of static nodes.
83 * @param distances euclidean distances from static nodes to mobile node.
84 * @param listener listener to be notified of events raised by this instance.
85 * @throws IllegalArgumentException if either positions or distances are null, don't have the same length or their
86 * length is smaller than required (2 points).
87 */
88 public NonLinearLeastSquaresLateration2DSolver(
89 final Point2D[] positions, final double[] distances, final LaterationSolverListener<Point2D> listener) {
90 super(positions, distances, listener);
91 }
92
93 /**
94 * Constructor.
95 *
96 * @param initialPosition initial position to start lateration solving.
97 * @param listener listener to be notified of events raised by this instance.
98 */
99 public NonLinearLeastSquaresLateration2DSolver(
100 final Point2D initialPosition, final LaterationSolverListener<Point2D> listener) {
101 super(initialPosition, listener);
102 }
103
104 /**
105 * Constructor.
106 *
107 * @param positions known positions of static nodes.
108 * @param distances euclidean distances from static nodes to mobile node.
109 * @param initialPosition initial position to start lateration solving.
110 * @param listener listener to be notified of events raised by this instance.
111 * @throws IllegalArgumentException if either positions or distances are null, don't have the same length or their
112 * length is smaller than required (2 points).
113 */
114 public NonLinearLeastSquaresLateration2DSolver(
115 final Point2D[] positions, final double[] distances, final Point2D initialPosition,
116 final LaterationSolverListener<Point2D> listener) {
117 super(positions, distances, initialPosition, listener);
118 }
119
120 /**
121 * Constructor.
122 *
123 * @param circles circles defining positions and distances.
124 * @throws IllegalArgumentException if circles is null or if length of circles array is less than 2.
125 */
126 public NonLinearLeastSquaresLateration2DSolver(final Circle[] circles) {
127 super();
128 internalSetCircles(circles);
129 }
130
131 /**
132 * Constructor.
133 *
134 * @param circles circles defining positions and distances.
135 * @param initialPosition initial position to start lateration solving.
136 * @throws IllegalArgumentException if circles is null or if length of circles array is less than 2.
137 */
138 public NonLinearLeastSquaresLateration2DSolver(final Circle[] circles, final Point2D initialPosition) {
139 super(initialPosition);
140 internalSetCircles(circles);
141 }
142
143 /**
144 * Constructor.
145 *
146 * @param circles circles defining positions and distances.
147 * @param listener listener to be notified of events raised by this instance.
148 * @throws IllegalArgumentException if circles is null or if length of circles array is less than 2.
149 */
150 public NonLinearLeastSquaresLateration2DSolver(
151 final Circle[] circles, final LaterationSolverListener<Point2D> listener) {
152 super(listener);
153 internalSetCircles(circles);
154 }
155
156 /**
157 * Constructor.
158 *
159 * @param circles circles defining positions and distances.
160 * @param initialPosition initial position to start lateration solving.
161 * @param listener listener to be notified of events raised by this instance.
162 * @throws IllegalArgumentException if circles is null or if length of circles array is less than 2.
163 */
164 public NonLinearLeastSquaresLateration2DSolver(
165 final Circle[] circles, final Point2D initialPosition, final LaterationSolverListener<Point2D> listener) {
166 super(initialPosition, listener);
167 internalSetCircles(circles);
168 }
169
170 /**
171 * Constructor.
172 *
173 * @param positions known positions of static nodes.
174 * @param distances euclidean distances from static nodes to mobile node.
175 * @param distanceStandardDeviations standard deviations of provided measured distances.
176 * @throws IllegalArgumentException if either positions, distances or standard deviations
177 * are null, don't have the same length of their length is smaller than required
178 * (2 points).
179 */
180 public NonLinearLeastSquaresLateration2DSolver(
181 final Point2D[] positions, final double[] distances, final double[] distanceStandardDeviations) {
182 super(positions, distances, distanceStandardDeviations);
183 }
184
185 /**
186 * Constructor.
187 *
188 * @param positions known positions of static nodes.
189 * @param distances euclidean distances from static nodes to mobile node.
190 * @param distanceStandardDeviations standard deviations of provided measured distances.
191 * @param initialPosition initial position to start lateration solving.
192 * @throws IllegalArgumentException if either positions, distances or standard deviations
193 * are null, don't have the same length of their length is smaller than required
194 * (2 points).
195 */
196 public NonLinearLeastSquaresLateration2DSolver(
197 final Point2D[] positions, final double[] distances, final double[] distanceStandardDeviations,
198 final Point2D initialPosition) {
199 super(positions, distances, distanceStandardDeviations, initialPosition);
200 }
201
202 /**
203 * Constructor.
204 *
205 * @param positions known positions of static nodes.
206 * @param distances euclidean distances from static nodes to mobile node.
207 * @param distanceStandardDeviations standard deviations of provided measured distances.
208 * @param listener listener to be notified of events raised by this instance.
209 * @throws IllegalArgumentException if either positions, distances or standard deviations
210 * are null, don't have the same length of their length is smaller than required
211 * (2 points).
212 */
213 public NonLinearLeastSquaresLateration2DSolver(
214 final Point2D[] positions, final double[] distances, final double[] distanceStandardDeviations,
215 final LaterationSolverListener<Point2D> listener) {
216 super(positions, distances, distanceStandardDeviations, listener);
217 }
218
219 /**
220 * Constructor.
221 *
222 * @param positions known positions of static nodes.
223 * @param distances euclidean distances from static nodes to mobile node.
224 * @param distanceStandardDeviations standard deviations of provided measured distances.
225 * @param initialPosition initial position to start lateration solving.
226 * @param listener listener to be notified of events raised by this instance.
227 * @throws IllegalArgumentException if either positions, distances or standard deviations
228 * are null, don't have the same length of their length is smaller than required
229 * (2 points).
230 */
231 public NonLinearLeastSquaresLateration2DSolver(
232 final Point2D[] positions, final double[] distances, final double[] distanceStandardDeviations,
233 final Point2D initialPosition, final LaterationSolverListener<Point2D> listener) {
234 super(positions, distances, distanceStandardDeviations, initialPosition, listener);
235 }
236
237 /**
238 * Constructor.
239 *
240 * @param circles circles defining positions and distances.
241 * @param distanceStandardDeviations standard deviations of provided measured distances.
242 * @throws IllegalArgumentException if circles is null or if length of circles array is less than 2.
243 */
244 public NonLinearLeastSquaresLateration2DSolver(
245 final Circle[] circles, final double[] distanceStandardDeviations) {
246 super();
247 internalSetCirclesAndStandardDeviations(circles, distanceStandardDeviations);
248 }
249
250 /**
251 * Constructor.
252 *
253 * @param circles circles defining positions and distances.
254 * @param distanceStandardDeviations standard deviations of provided measured distances.
255 * @param initialPosition initial position to start lateration solving.
256 * @throws IllegalArgumentException if circles is null or if length of circles array is less than 2.
257 */
258 public NonLinearLeastSquaresLateration2DSolver(
259 final Circle[] circles, final double[] distanceStandardDeviations, final Point2D initialPosition) {
260 super(initialPosition);
261 internalSetCirclesAndStandardDeviations(circles, distanceStandardDeviations);
262 }
263
264 /**
265 * Constructor.
266 *
267 * @param circles circles defining positions and distances.
268 * @param distanceStandardDeviations standard deviations of provided measured distances.
269 * @param listener listener to be notified of events raised by this instance.
270 * @throws IllegalArgumentException if circles is null or if length of circles array is less than 2.
271 */
272 public NonLinearLeastSquaresLateration2DSolver(
273 final Circle[] circles, final double[] distanceStandardDeviations,
274 final LaterationSolverListener<Point2D> listener) {
275 super(listener);
276 internalSetCirclesAndStandardDeviations(circles, distanceStandardDeviations);
277 }
278
279 /**
280 * Constructor.
281 *
282 * @param circles circles defining positions and distances.
283 * @param distanceStandardDeviations standard deviations of provided measured distances.
284 * @param initialPosition initial position to start lateration solving.
285 * @param listener listener to be notified of events raised by this instance.
286 * @throws IllegalArgumentException if circles is null or if length of circles array is less than 2.
287 */
288 public NonLinearLeastSquaresLateration2DSolver(
289 final Circle[] circles, final double[] distanceStandardDeviations, final Point2D initialPosition,
290 final LaterationSolverListener<Point2D> listener) {
291 super(initialPosition, listener);
292 internalSetCirclesAndStandardDeviations(circles, distanceStandardDeviations);
293 }
294
295 /**
296 * Gets circles defined by provided positions and distances.
297 *
298 * @return circles defined by provided positions and distances.
299 */
300 public Circle[] getCircles() {
301 if (positions == null) {
302 return null;
303 }
304
305 final var result = new Circle[positions.length];
306
307 for (var i = 0; i < positions.length; i++) {
308 result[i] = new Circle(positions[i], distances[i]);
309 }
310 return result;
311 }
312
313 /**
314 * Sets circles defining positions and euclidean distances.
315 *
316 * @param circles circles defining positions and distances.
317 * @throws IllegalArgumentException if circles is null or length of array of circles
318 * is less than 2.
319 * @throws LockedException if instance is busy solving the lateration problem.
320 */
321 public void setCircles(final Circle[] circles) throws LockedException {
322 if (isLocked()) {
323 throw new LockedException();
324 }
325 internalSetCircles(circles);
326 }
327
328 /**
329 * Sets circles defining positions and Euclidean distances along with the standard
330 * deviations of provided circles radii.
331 *
332 * @param circles circles defining positions and distances.
333 * @param radiusStandardDeviations standard deviations of circles radii.
334 * @throws IllegalArgumentException if circles is null, length of arrays is less than
335 * 2 or don't have the same length.
336 * @throws LockedException if instance is busy solving the lateration problem.
337 */
338 public void setCirclesAndStandardDeviations(final Circle[] circles, final double[] radiusStandardDeviations)
339 throws LockedException {
340 if (isLocked()) {
341 throw new LockedException();
342 }
343 internalSetCirclesAndStandardDeviations(circles, radiusStandardDeviations);
344 }
345
346 /**
347 * Gets number of dimensions of provided points.
348 *
349 * @return always returns 2 dimensions.
350 */
351 @Override
352 public int getNumberOfDimensions() {
353 return Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH;
354 }
355
356 /**
357 * Minimum required number of positions and distances.
358 * At least 3 positions and distances will be required to solve a 2D problem.
359 *
360 * @return minimum required number of positions and distances.
361 */
362 @Override
363 public int getMinRequiredPositionsAndDistances() {
364 return Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH + 1;
365 }
366
367 /**
368 * Gets estimated position.
369 *
370 * @return estimated position.
371 */
372 @Override
373 public Point2D getEstimatedPosition() {
374 if (estimatedPositionCoordinates == null) {
375 return null;
376 }
377
378 final var position = new InhomogeneousPoint2D();
379 getEstimatedPosition(position);
380 return position;
381 }
382
383 /**
384 * Internally sets circles defining positions and Euclidean distances.
385 *
386 * @param circles circles defining positions and distances.
387 * @throws IllegalArgumentException if circles is null or length of array of circles
388 * is less than 2.
389 */
390 private void internalSetCircles(final Circle[] circles) {
391 if (circles == null || circles.length < getMinRequiredPositionsAndDistances()) {
392 throw new IllegalArgumentException();
393 }
394
395 final var positions = new Point2D[circles.length];
396 final var distances = new double[circles.length];
397 for (var i = 0; i < circles.length; i++) {
398 final var circle = circles[i];
399 positions[i] = circle.getCenter();
400 distances[i] = circle.getRadius();
401 }
402
403 internalSetPositionsAndDistances(positions, distances);
404 }
405
406 /**
407 * Internally sets circles defining positions and Euclidean distances along with the standard
408 * deviations of provided circles radii.
409 *
410 * @param circles circles defining positions and distances.
411 * @param radiusStandardDeviations standard deviations of circles radii.
412 * @throws IllegalArgumentException if circles is null, length of arrays is less than
413 * 2 or don't have the same length.
414 */
415 private void internalSetCirclesAndStandardDeviations(
416 final Circle[] circles, final double[] radiusStandardDeviations) {
417 if (circles == null || circles.length < getMinRequiredPositionsAndDistances()) {
418 throw new IllegalArgumentException();
419 }
420
421 if (radiusStandardDeviations == null) {
422 throw new IllegalArgumentException();
423 }
424
425 if (radiusStandardDeviations.length != circles.length) {
426 throw new IllegalArgumentException();
427 }
428
429 final var positions = new Point2D[circles.length];
430 final var distances = new double[circles.length];
431 for (var i = 0; i < circles.length; i++) {
432 final var circle = circles[i];
433 positions[i] = circle.getCenter();
434 distances[i] = circle.getRadius();
435 }
436
437 internalSetPositionsDistancesAndStandardDeviations(positions, distances, radiusStandardDeviations);
438 }
439 }