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.RangingReading;
25 import com.irurueta.navigation.lateration.HomogeneousLinearLeastSquaresLateration2DSolver;
26 import com.irurueta.navigation.lateration.InhomogeneousLinearLeastSquaresLateration2DSolver;
27
28 import java.util.List;
29
30 /**
31 * Linearly estimates 2D position using located radio sources and their ranging readings at
32 * unknown locations.
33 * These kind of estimators can be used to determine the 2D position of a given device by
34 * getting ranging readings at an unknown location of different radio sources whose
35 * locations are known.
36 */
37 public class LinearRangingPositionEstimator2D extends LinearRangingPositionEstimator<Point2D> {
38
39 /**
40 * Constructor.
41 */
42 public LinearRangingPositionEstimator2D() {
43 super();
44 initialize();
45 }
46
47 /**
48 * Constructor.
49 *
50 * @param sources located radio sources used for lateration.
51 * @throws IllegalArgumentException if provided sources is null or the number of
52 * provided sources is less than the required
53 * minimum.
54 */
55 public LinearRangingPositionEstimator2D(final List<? extends RadioSourceLocated<Point2D>> sources) {
56 super();
57 initialize();
58 internalSetSources(sources);
59 }
60
61 /**
62 * Constructor.
63 *
64 * @param fingerprint fingerprint containing ranging readings at an unknown location
65 * for provided located radio sources.
66 * @throws IllegalArgumentException if provided fingerprint is null.
67 */
68 public LinearRangingPositionEstimator2D(
69 final Fingerprint<? extends RadioSource, ? extends RangingReading<? 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 ranging readings at an unknown location
80 * for provided located radio sources.
81 * @throws IllegalArgumentException if either provided sources or fingerprint is null
82 * or the number of provided sources is less than
83 * the required minimum.
84 */
85 public LinearRangingPositionEstimator2D(
86 final List<? extends RadioSourceLocated<Point2D>> sources,
87 final Fingerprint<? extends RadioSource, ? extends RangingReading<? extends RadioSource>> fingerprint) {
88 super();
89 initialize();
90 internalSetSources(sources);
91 internalSetFingerprint(fingerprint);
92 }
93
94 /**
95 * Constructor.
96 *
97 * @param listener listener in charge of handling events.
98 */
99 public LinearRangingPositionEstimator2D(final RangingPositionEstimatorListener<Point2D> listener) {
100 super(listener);
101 initialize();
102 }
103
104 /**
105 * Constructor.
106 *
107 * @param sources located radio sources used for lateration.
108 * @param listener listener in charge of handling events.
109 * @throws IllegalArgumentException if provided sources is null or the number of
110 * provided sources is less than the required
111 * minimum.
112 */
113 public LinearRangingPositionEstimator2D(
114 final List<? extends RadioSourceLocated<Point2D>> sources,
115 final RangingPositionEstimatorListener<Point2D> listener) {
116 super(listener);
117 initialize();
118 internalSetSources(sources);
119 }
120
121 /**
122 * Constructor.
123 *
124 * @param fingerprint fingerprint containing ranging readings at an unknown
125 * location for provided location radio sources.
126 * @param listener listener in charge of handling events.
127 * @throws IllegalArgumentException if provided fingerprint is null.
128 */
129 public LinearRangingPositionEstimator2D(
130 final Fingerprint<? extends RadioSource, ? extends RangingReading<? extends RadioSource>> fingerprint,
131 final RangingPositionEstimatorListener<Point2D> listener) {
132 super(listener);
133 initialize();
134 internalSetFingerprint(fingerprint);
135 }
136
137 /**
138 * Constructor.
139 *
140 * @param sources located radio sources used for lateration.
141 * @param fingerprint fingerprint containing ranging readings at an unknown
142 * location for provided located radio sources.
143 * @param listener listener in charge of handling events.
144 * @throws IllegalArgumentException if either provided sources or fingerprint is
145 * null or the number of provided sources is less
146 * than the required minimum.
147 */
148 public LinearRangingPositionEstimator2D(
149 final List<? extends RadioSourceLocated<Point2D>> sources,
150 final Fingerprint<? extends RadioSource, ? extends RangingReading<? extends RadioSource>> fingerprint,
151 final RangingPositionEstimatorListener<Point2D> listener) {
152 super(listener);
153 initialize();
154 internalSetSources(sources);
155 internalSetFingerprint(fingerprint);
156 }
157
158 /**
159 * Gets estimated position.
160 *
161 * @return estimated position.
162 */
163 @Override
164 public Point2D getEstimatedPosition() {
165 if (estimatedPositionCoordinates == null) {
166 return null;
167 }
168
169 final var result = new InhomogeneousPoint2D();
170 getEstimatedPosition(result);
171 return result;
172 }
173
174 /**
175 * Sets positions and distances on internal lateration solver.
176 *
177 * @param positions positions to be set.
178 * @param distances distances to be set.
179 * @throws IllegalArgumentException if something fails.
180 */
181 @Override
182 @SuppressWarnings("Duplicates")
183 protected void setPositionsAndDistances(final List<Point2D> positions, final List<Double> distances) {
184 final var size = positions.size();
185 Point2D[] positionsArray = new InhomogeneousPoint2D[size];
186 positionsArray = positions.toArray(positionsArray);
187
188 final var distancesArray = new double[size];
189 for (var i = 0; i < size; i++) {
190 distancesArray[i] = distances.get(i);
191 }
192
193 try {
194 homogeneousTrilaterationSolver.setPositionsAndDistances(positionsArray, distancesArray);
195 inhomogeneousTrilaterationSolver.setPositionsAndDistances(positionsArray, distancesArray);
196 } catch (final LockedException e) {
197 throw new IllegalArgumentException(e);
198 }
199 }
200
201 /**
202 * Initializes lateration solver.
203 */
204 private void initialize() {
205 homogeneousTrilaterationSolver = new HomogeneousLinearLeastSquaresLateration2DSolver(laterationSolverListener);
206 inhomogeneousTrilaterationSolver = new InhomogeneousLinearLeastSquaresLateration2DSolver(
207 laterationSolverListener);
208 }
209 }