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.algebra.NonSymmetricPositiveDefiniteMatrixException;
19 import com.irurueta.geometry.Accuracy;
20 import com.irurueta.geometry.Point;
21 import com.irurueta.navigation.LockedException;
22 import com.irurueta.navigation.NotReadyException;
23 import com.irurueta.navigation.indoor.RadioSource;
24 import com.irurueta.navigation.indoor.RangingReadingLocated;
25 import com.irurueta.navigation.lateration.HomogeneousLinearLeastSquaresLaterationSolver;
26 import com.irurueta.navigation.lateration.InhomogeneousLinearLeastSquaresLaterationSolver;
27 import com.irurueta.navigation.lateration.LaterationException;
28 import com.irurueta.navigation.lateration.NonLinearLeastSquaresLaterationSolver;
29
30 import java.util.ArrayList;
31 import java.util.List;
32
33 /**
34 * Estimates position of a radio source (e.g. Wi-Fi access point or bluetooth beacon)
35 * by using ranging measurements.
36 * Ranging measurements can be obtained by protocols such as ieee 802.11mc (Wi-Fi RTT) which
37 * measures travel time of signal and converts the result into distances by taking into
38 * account the speed of light as the propagation speed.
39 */
40 public abstract class RangingRadioSourceEstimator<S extends RadioSource, P extends Point<P>>
41 extends RadioSourceEstimator<P, RangingReadingLocated<S, P>, RangingRadioSourceEstimatorListener<S, P>> {
42
43 /**
44 * Indicates that by default position covariances of readings must be taken into account to increase
45 * the amount of standard deviation of each ranging measure by the amount of position standard deviation
46 * assuming that both measures are statistically independent.
47 */
48 public static final boolean DEFAULT_USE_READING_POSITION_COVARIANCES = true;
49
50 /**
51 * Indicates that by default an homogeneous linear solver is used to estimate an
52 * initial position.
53 */
54 public static final boolean DEFAULT_USE_HOMOGENEOUS_LINEAR_SOLVER = true;
55
56 /**
57 * Internal homogeneous linear solver to find radio source position when no initial
58 * position is provided.
59 */
60 protected HomogeneousLinearLeastSquaresLaterationSolver<P> homogeneousLinearSolver;
61
62 /**
63 * Internal inhomogeneous linear solver to find radio source position when no initial
64 * position is provided.
65 */
66 protected InhomogeneousLinearLeastSquaresLaterationSolver<P> inhomogeneousLinearSolver;
67
68 /**
69 * Internal non-linear solver to estimate radio source position and covariance
70 * for an initial provided or estimated position.
71 */
72 protected NonLinearLeastSquaresLaterationSolver<P> nonLinearSolver;
73
74 /**
75 * Contains accuracy of a reading position.
76 * This is used internally to compute additional distance standard deviation due to position
77 * accuracy.
78 */
79 protected Accuracy accuracy;
80
81 /**
82 * Initial position to start the estimation of radio source position.
83 */
84 protected P initialPosition;
85
86 /**
87 * Indicates whether non-linear solver is enabled.
88 * If disabled a linear solver is always used, initial position ignored and
89 * covariance is not computed.
90 */
91 protected boolean nonLinearSolverEnabled = true;
92
93 /**
94 * Indicates whether an homogeneous linear solver is used to estimate an initial
95 * position.
96 */
97 protected boolean useHomogeneousLinearSolver = DEFAULT_USE_HOMOGENEOUS_LINEAR_SOLVER;
98
99 /**
100 * Indicates whether position covariances of readings must be taken into account to increase
101 * the amount of standard deviation of each ranging measure by the amount of position standard deviation
102 * assuming that both measures are statistically independent.
103 */
104 protected boolean useReadingPositionCovariances = DEFAULT_USE_READING_POSITION_COVARIANCES;
105
106 /**
107 * Constructor.
108 */
109 protected RangingRadioSourceEstimator() {
110 super();
111 }
112
113 /**
114 * Constructor.
115 * Sets radio signal ranging readings belonging to the same radio source.
116 *
117 * @param readings radio signal ranging readings belonging to the same
118 * radio source.
119 * @throws IllegalArgumentException if readings are not valid.
120 */
121 protected RangingRadioSourceEstimator(final List<? extends RangingReadingLocated<S, P>> readings) {
122 super(readings);
123 }
124
125 /**
126 * Constructor.
127 *
128 * @param listener listener in charge of attending events raised by this instance.
129 */
130 protected RangingRadioSourceEstimator(final RangingRadioSourceEstimatorListener<S, P> listener) {
131 super(listener);
132 }
133
134 /**
135 * Constructor.
136 * Sets radio signal readings belonging to the same radio source.
137 *
138 * @param readings radio signal readings belonging to the same radio source.
139 * @param listener listener in charge of attending events raised by this instance.
140 * @throws IllegalArgumentException if readings are not valid.
141 */
142 protected RangingRadioSourceEstimator(
143 final List<? extends RangingReadingLocated<S, P>> readings,
144 final RangingRadioSourceEstimatorListener<S, P> listener) {
145 super(readings, listener);
146 }
147
148 /**
149 * Constructor.
150 *
151 * @param initialPosition initial position to start the estimation or radio
152 * source position.
153 */
154 protected RangingRadioSourceEstimator(final P initialPosition) {
155 this.initialPosition = initialPosition;
156 }
157
158 /**
159 * Constructor.
160 * Sets radio signal readings belonging to the same radio source.
161 *
162 * @param readings radio signal readings belonging to the same radio source.
163 * @param initialPosition initial position to start the estimation of radio
164 * source position.
165 * @throws IllegalArgumentException if readings are not valid.
166 */
167 protected RangingRadioSourceEstimator(
168 final List<? extends RangingReadingLocated<S, P>> readings, final P initialPosition) {
169 super(readings);
170 this.initialPosition = initialPosition;
171 }
172
173 /**
174 * Constructor.
175 *
176 * @param initialPosition initial position to start the estimation of radio
177 * source position.
178 * @param listener listener in charge of attending events raised by this instance.
179 */
180 protected RangingRadioSourceEstimator(
181 final P initialPosition, final RangingRadioSourceEstimatorListener<S, P> listener) {
182 super(listener);
183 this.initialPosition = initialPosition;
184 }
185
186 /**
187 * Constructor.
188 * Sets radio signal ranging readings belonging to the same radio source.
189 *
190 * @param readings radio signal ranging readings belonging to the same radio source.
191 * @param initialPosition initial position to start the estimation of radio source
192 * position.
193 * @param listener listener in charge of attending events raised by this instance.
194 * @throws IllegalArgumentException if readings are not valid.
195 */
196 protected RangingRadioSourceEstimator(
197 final List<? extends RangingReadingLocated<S, P>> readings, final P initialPosition,
198 final RangingRadioSourceEstimatorListener<S, P> listener) {
199 super(readings, listener);
200 this.initialPosition = initialPosition;
201 }
202
203 /**
204 * Gets initial position to start the non-linear estimation of radio source position.
205 * If not defined, a linear solution is found instead.
206 *
207 * @return initial position.
208 */
209 public P getInitialPosition() {
210 return initialPosition;
211 }
212
213 /**
214 * Sets initial position to start the non-linear estimation of radio source position.
215 * If not defined, a linear solution is found instead.
216 *
217 * @param initialPosition initial position to start the estimation of radio source
218 * position or null.
219 * @throws LockedException if estimator is locked.
220 */
221 public void setInitialPosition(final P initialPosition) throws LockedException {
222 if (isLocked()) {
223 throw new LockedException();
224 }
225 this.initialPosition = initialPosition;
226 }
227
228 /**
229 * Indicates whether non-linear solver is enabled.
230 * If disabled a linear solver is always used, initial position ignored and
231 * covariance is not computed.
232 *
233 * @return true if non-linear solver is enabled, false otherwise.
234 */
235 public boolean isNonLinearSolverEnabled() {
236 return nonLinearSolverEnabled;
237 }
238
239 /**
240 * Specifies whether non-linear solver is enabled.
241 * If disabled a linear solver is always used, initial position ignored and
242 * covariance is not computed.
243 *
244 * @param nonLinearSolverEnabled true if non-linear solver is enabled,
245 * false otherwise.
246 * @throws LockedException if estimator is locked.
247 */
248 public void setNonLinearSolverEnabled(final boolean nonLinearSolverEnabled) throws LockedException {
249 if (isLocked()) {
250 throw new LockedException();
251 }
252 this.nonLinearSolverEnabled = nonLinearSolverEnabled;
253 }
254
255 /**
256 * Indicates whether an homogeneous linear solver is used to estimate an initial
257 * position.
258 *
259 * @return true if homogeneous linear solver is used, false if an inhomogeneous linear
260 * one is used instead.
261 */
262 public boolean isHomogeneousLinearSolverUsed() {
263 return useHomogeneousLinearSolver;
264 }
265
266 /**
267 * Specifies whether an homogeneous linear solver is used to estimate an initial
268 * position.
269 *
270 * @param useHomogeneousLinearSolver true if homogeneous linear solver is used, false
271 * if an inhomogeneous linear one is used instead.
272 * @throws LockedException if estimator is locked.
273 */
274 public void setHomogeneousLinearSolverUsed(final boolean useHomogeneousLinearSolver) throws LockedException {
275 if (isLocked()) {
276 throw new LockedException();
277 }
278
279 this.useHomogeneousLinearSolver = useHomogeneousLinearSolver;
280 }
281
282
283 /**
284 * Indicates whether position covariances of readings must be taken into account to increase
285 * the amount of standard deviation of each ranging measure by the amount of position standard
286 * deviation assuming that both measures are statistically independent.
287 *
288 * @return true to take into account reading position covariances, false otherwise.
289 */
290 public boolean getUseReadingPositionCovariance() {
291 return useReadingPositionCovariances;
292 }
293
294 /**
295 * Specifies whether position covariances of readings must be taken into account to increase
296 * the amount of standard deviation of each ranging measure by the amount of position standard
297 * deviation assuming that both measures are statistically independent.
298 *
299 * @param useReadingPositionCovariances true to take into account reading position covariances, false
300 * otherwise.
301 * @throws LockedException if estimator is locked.
302 */
303 public void setUseReadingPositionCovariances(final boolean useReadingPositionCovariances) throws LockedException {
304 if (isLocked()) {
305 throw new LockedException();
306 }
307 this.useReadingPositionCovariances = useReadingPositionCovariances;
308 }
309
310 /**
311 * Indicates whether this instance is ready to start the estimation.
312 *
313 * @return true if this instance is ready, false otherwise.
314 */
315 @Override
316 public boolean isReady() {
317 // readings must be valid
318 return areValidReadings(readings);
319 }
320
321 /**
322 * Estimate position of radio source.
323 *
324 * @throws RadioSourceEstimationException if estimation fails.
325 * @throws NotReadyException if estimator is not ready.
326 * @throws LockedException if estimator is locked.
327 */
328 @Override
329 public void estimate() throws RadioSourceEstimationException, NotReadyException, LockedException {
330 if (isLocked()) {
331 throw new LockedException();
332 }
333 if (!isReady()) {
334 throw new NotReadyException();
335 }
336
337 try {
338 locked = true;
339
340 if (listener != null) {
341 listener.onEstimateStart(this);
342 }
343
344 buildSolversIfNeeded();
345 buildPositionsDistancesAndDistanceStandardDeviations();
346
347 if (((useHomogeneousLinearSolver && homogeneousLinearSolver != null)
348 || (!useHomogeneousLinearSolver && inhomogeneousLinearSolver != null))
349 && (initialPosition == null || !nonLinearSolverEnabled)) {
350 // if no initial position is provided, use linear solver to estimate one
351 if (useHomogeneousLinearSolver) {
352 homogeneousLinearSolver.solve();
353 initialPosition = homogeneousLinearSolver.getEstimatedPosition();
354 } else {
355 inhomogeneousLinearSolver.solve();
356 initialPosition = inhomogeneousLinearSolver.getEstimatedPosition();
357 }
358 }
359
360 if (nonLinearSolver != null && nonLinearSolverEnabled) {
361 nonLinearSolver.setInitialPosition(initialPosition);
362 nonLinearSolver.solve();
363
364 // get position and covariance
365 estimatedPositionCoordinates = nonLinearSolver.getEstimatedPositionCoordinates();
366 estimatedPositionCovariance = estimatedCovariance = nonLinearSolver.getCovariance();
367 } else {
368 // non-linear solver disabled
369 if (useHomogeneousLinearSolver) {
370 estimatedPositionCoordinates = homogeneousLinearSolver != null
371 ? homogeneousLinearSolver.getEstimatedPositionCoordinates() : null;
372 } else {
373 estimatedPositionCoordinates = inhomogeneousLinearSolver != null
374 ? inhomogeneousLinearSolver.getEstimatedPositionCoordinates() : null;
375 }
376 estimatedPositionCovariance = estimatedCovariance = null;
377 }
378
379 if (listener != null) {
380 listener.onEstimateEnd(this);
381 }
382
383 } catch (final LaterationException e) {
384 throw new RadioSourceEstimationException(e);
385 } finally {
386 locked = false;
387 }
388 }
389
390 /**
391 * Builds an instance of a linear lateration solver if needed.
392 */
393 protected abstract void buildLinearSolverIfNeeded();
394
395 /**
396 * Builds an instance of a non-linear lateration solver if needed.
397 */
398 protected abstract void buildNonLinearSolverIfNeeded();
399
400 /**
401 * Build an instance of accuracy if needed.
402 */
403 protected abstract void buildAccuracyIfNeeded();
404
405 /**
406 * Sets positions, distances and standard deviations of distances on internal
407 * lateration solver.
408 *
409 * @param positions positions to be set.
410 * @param distances distances to be set.
411 * @param distanceStandardDeviations standard deviations of distances to be set or
412 * null.
413 * @throws LockedException if solvers are locked.
414 */
415 protected abstract void setPositionsDistancesAndDistanceStandardDeviations(
416 final List<P> positions, List<Double> distances, final List<Double> distanceStandardDeviations)
417 throws LockedException;
418
419 /**
420 * Build instances of lateration solvers if needed.
421 */
422 private void buildSolversIfNeeded() {
423 buildLinearSolverIfNeeded();
424 buildNonLinearSolverIfNeeded();
425 buildAccuracyIfNeeded();
426 }
427
428 /**
429 * Builds positions, distances and standard deviations of distances for the
430 * internal lateration solver.
431 *
432 * @throws LockedException if solvers are locked.
433 */
434 private void buildPositionsDistancesAndDistanceStandardDeviations() throws LockedException {
435 final var min = getMinReadings();
436 if (readings == null || readings.size() < min) {
437 return;
438 }
439
440 final var positions = new ArrayList<P>();
441 final var distances = new ArrayList<Double>();
442 final var distanceStandardDeviations = new ArrayList<Double>();
443
444 for (final var reading : readings) {
445 final var position = reading.getPosition();
446 if (position == null) {
447 return;
448 }
449
450 var positionDistanceStandardDeviation = 0.0;
451 if (useReadingPositionCovariances && accuracy != null && reading.getPositionCovariance() != null) {
452 try {
453 accuracy.setCovarianceMatrix(reading.getPositionCovariance());
454 positionDistanceStandardDeviation = accuracy.getAverageAccuracy();
455 } catch (final NonSymmetricPositiveDefiniteMatrixException e) {
456 positionDistanceStandardDeviation = 0.0;
457 }
458 }
459
460 final var distance = reading.getDistance();
461 var distanceStandardDeviation = reading.getDistanceStandardDeviation();
462 if (distanceStandardDeviation == null) {
463 distanceStandardDeviation = NonLinearLeastSquaresLaterationSolver.DEFAULT_DISTANCE_STANDARD_DEVIATION;
464 }
465
466 if (useReadingPositionCovariances) {
467 // assuming that ranging measure and position measure are statistically independent, the
468 // resulting variance would be the sum of their variances, when the resulting standard
469 // deviation is the square root of the resulting variance (which is the sum of the square of
470 // the standard deviations)
471 distanceStandardDeviation = Math.sqrt(Math.pow(distanceStandardDeviation, 2.0)
472 + Math.pow(positionDistanceStandardDeviation, 2.0));
473 }
474
475 positions.add(position);
476 distances.add(distance);
477 distanceStandardDeviations.add(distanceStandardDeviation);
478 }
479
480 setPositionsDistancesAndDistanceStandardDeviations(positions, distances, distanceStandardDeviations);
481 }
482 }