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.indoor.radiosource;
17
18 import com.irurueta.geometry.Point;
19 import com.irurueta.navigation.LockedException;
20 import com.irurueta.navigation.indoor.RadioSource;
21 import com.irurueta.navigation.indoor.RangingReadingLocated;
22 import com.irurueta.numerical.robust.RobustEstimatorMethod;
23
24 import java.util.List;
25
26 /**
27 * This is an abstract class to robustly estimate position of a radio source (e.g. Wi-Fi
28 * access point or bluetooth beacon), by discarding outliers.
29 *
30 * @param <S> a {@link RadioSource} type.
31 * @param <P> a {@link Point} type.
32 */
33 public abstract class RobustRangingRadioSourceEstimator<S extends RadioSource, P extends Point<P>> extends
34 RobustRadioSourceEstimator<P, RangingReadingLocated<S, P>, RobustRangingRadioSourceEstimatorListener<S, P>> {
35
36 /**
37 * Indicates that by default position covariances of readings must be taken into account to increase
38 * the amount of standard deviation of each ranging measure by the amount of position standard deviation
39 * assuming that both measures are statistically independent.
40 */
41 public static final boolean DEFAULT_USE_READING_POSITION_COVARIANCES = true;
42
43 /**
44 * Initial position to start the estimation of radio source position.
45 */
46 protected P initialPosition;
47
48 /**
49 * Indicates whether position covariances of readings must be taken into account to increase
50 * the amount of standard deviation of each ranging measure by the amount of position standard deviation
51 * assuming that both measures are statistically independent.
52 */
53 protected boolean useReadingPositionCovariances = DEFAULT_USE_READING_POSITION_COVARIANCES;
54
55 /**
56 * Constructor.
57 */
58 protected RobustRangingRadioSourceEstimator() {
59 super();
60 }
61
62 /**
63 * Constructor.
64 * Sets radio signal ranging readings belonging to the same radio source.
65 *
66 * @param readings radio signal ranging readings belonging to the same
67 * radio source.
68 * @throws IllegalArgumentException if readings are not valid.
69 */
70 protected RobustRangingRadioSourceEstimator(final List<? extends RangingReadingLocated<S, P>> readings) {
71 super(readings);
72 }
73
74 /**
75 * Constructor.
76 *
77 * @param listener listener in charge of attending events raised by this instance.
78 */
79 protected RobustRangingRadioSourceEstimator(final RobustRangingRadioSourceEstimatorListener<S, P> listener) {
80 super(listener);
81 }
82
83 /**
84 * Constructor.
85 * Sets radio signal readings belonging to the same radio source.
86 *
87 * @param readings radio signal readings belonging to the same radio source.
88 * @param listener listener in charge of attending events raised by this instance.
89 * @throws IllegalArgumentException if readings are not valid.
90 */
91 protected RobustRangingRadioSourceEstimator(
92 final List<? extends RangingReadingLocated<S, P>> readings,
93 final RobustRangingRadioSourceEstimatorListener<S, P> listener) {
94 super(readings, listener);
95 }
96
97 /**
98 * Constructor.
99 *
100 * @param initialPosition initial position to start the estimation or radio
101 * source position.
102 */
103 protected RobustRangingRadioSourceEstimator(final P initialPosition) {
104 this.initialPosition = initialPosition;
105 }
106
107 /**
108 * Constructor.
109 * Sets radio signal readings belonging to the same radio source.
110 *
111 * @param readings radio signal readings belonging to the same radio source.
112 * @param initialPosition initial position to start the estimation of radio
113 * source position.
114 * @throws IllegalArgumentException if readings are not valid.
115 */
116 protected RobustRangingRadioSourceEstimator(
117 final List<? extends RangingReadingLocated<S, P>> readings, final P initialPosition) {
118 super(readings);
119 this.initialPosition = initialPosition;
120 }
121
122 /**
123 * Constructor.
124 *
125 * @param initialPosition initial position to start the estimation of radio
126 * source position.
127 * @param listener listener in charge of attending events raised by this instance.
128 */
129 protected RobustRangingRadioSourceEstimator(
130 final P initialPosition, final RobustRangingRadioSourceEstimatorListener<S, P> listener) {
131 super(listener);
132 this.initialPosition = initialPosition;
133 }
134
135 /**
136 * Constructor.
137 * Sets radio signal ranging readings belonging to the same radio source.
138 *
139 * @param readings radio signal ranging readings belonging to the same radio source.
140 * @param initialPosition initial position to start the estimation of radio source
141 * position.
142 * @param listener listener in charge of attending events raised by this instance.
143 * @throws IllegalArgumentException if readings are not valid.
144 */
145 protected RobustRangingRadioSourceEstimator(
146 final List<? extends RangingReadingLocated<S, P>> readings, final P initialPosition,
147 final RobustRangingRadioSourceEstimatorListener<S, P> listener) {
148 super(readings, listener);
149 this.initialPosition = initialPosition;
150 }
151
152 /**
153 * Gets initial position to start the non-linear estimation of radio source position.
154 * If not defined, a linear solution is found instead.
155 *
156 * @return initial position.
157 */
158 public P getInitialPosition() {
159 return initialPosition;
160 }
161
162 /**
163 * Sets initial position to start the non-linear estimation of radio source position.
164 * If not defined, a linear solution is found instead.
165 *
166 * @param initialPosition initial position to start the estimation of radio source
167 * position or null.
168 * @throws LockedException if estimator is locked.
169 */
170 public void setInitialPosition(final P initialPosition) throws LockedException {
171 if (isLocked()) {
172 throw new LockedException();
173 }
174 this.initialPosition = initialPosition;
175 }
176
177 /**
178 * Indicates whether position covariances of readings must be taken into account to increase
179 * the amount of standard deviation of each ranging measure by the amount of position standard
180 * deviation assuming that both measures are statistically independent.
181 *
182 * @return true to take into account reading position covariances, false otherwise.
183 */
184 public boolean getUseReadingPositionCovariance() {
185 return useReadingPositionCovariances;
186 }
187
188 /**
189 * Specifies whether position covariances of readings must be taken into account to increase
190 * the amount of standard deviation of each ranging measure by the amount of position standard
191 * deviation assuming that both measures are statistically independent.
192 *
193 * @param useReadingPositionCovariances true to take into account reading position covariances, false
194 * otherwise.
195 * @throws LockedException if estimator is locked.
196 */
197 public void setUseReadingPositionCovariances(boolean useReadingPositionCovariances) throws LockedException {
198 if (isLocked()) {
199 throw new LockedException();
200 }
201 this.useReadingPositionCovariances = useReadingPositionCovariances;
202 }
203
204 /**
205 * Indicates whether this instance is ready to start the estimation.
206 *
207 * @return true if this instance is ready, false otherwise.
208 */
209 @Override
210 public boolean isReady() {
211 //readings must be valid
212 return areValidReadings(readings);
213 }
214
215 /**
216 * Indicates whether an homogeneous linear solver is used to estimate an initial
217 * position.
218 *
219 * @return true if homogeneous linear solver is used, false if an inhomogeneous linear
220 * one is used instead.
221 */
222 public abstract boolean isHomogeneousLinearSolverUsed();
223
224 /**
225 * Specifies whether an homogeneous linear solver is used to estimate an initial
226 * position.
227 *
228 * @param useHomogeneousLinearSolver true if homogeneous linear solver is used, false
229 * if an inhomogeneous linear one is used instead.
230 * @throws LockedException if estimator is locked.
231 */
232 public abstract void setHomogeneousLinearSolverUsed(final boolean useHomogeneousLinearSolver)
233 throws LockedException;
234
235 /**
236 * Returns method being used for robust estimation.
237 *
238 * @return method being used for robust estimation.
239 */
240 public abstract RobustEstimatorMethod getMethod();
241
242 /**
243 * Solves preliminary solution for a subset of samples.
244 *
245 * @param samplesIndices indices of subset samples.
246 * @param solutions instance where solution will be stored.
247 */
248 protected abstract void solvePreliminarySolutions(final int[] samplesIndices, final List<Solution<P>> solutions);
249
250 /**
251 * Estimates residual for a solution obtained for a subset of samples.
252 *
253 * @param currentEstimation solution obtained for a subset of samples.
254 * @param i i-th fingerprint to obtain residual for.
255 * @return difference between measured and expected RSSI value.
256 */
257 protected double residual(final Solution<P> currentEstimation, final int i) {
258 final var reading = readings.get(i);
259 final var distance = reading.getDistance();
260
261 // get distance from estimated radio source position and reading position
262 final var readingPosition = reading.getPosition();
263 final var radioSourcePosition = currentEstimation.getEstimatedPosition();
264
265 return Math.abs(radioSourcePosition.distanceTo(readingPosition) - distance);
266 }
267
268 /**
269 * Contains a solution obtained during robust estimation for a subset of
270 * samples.
271 *
272 * @param <P> a {@link Point} type.
273 */
274 protected static class Solution<P extends Point<?>> {
275 /**
276 * Estimated position for a subset of samples.
277 */
278 private final P mEstimatedPosition;
279
280 /**
281 * Constructor.
282 *
283 * @param estimatedPosition estimated position for a subset of samples.
284 */
285 public Solution(final P estimatedPosition) {
286 mEstimatedPosition = estimatedPosition;
287 }
288
289 /**
290 * Gets estimated position for a subset of samples.
291 *
292 * @return estimated position for a subset of samples.
293 */
294 public P getEstimatedPosition() {
295 return mEstimatedPosition;
296 }
297 }
298 }