1 /*
2 * Copyright (C) 2021 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.inertial.calibration.intervals.thresholdfactor;
17
18 import com.irurueta.navigation.LockedException;
19 import com.irurueta.navigation.NavigationException;
20 import com.irurueta.navigation.NotReadyException;
21 import com.irurueta.navigation.inertial.calibration.BodyKinematicsSequence;
22 import com.irurueta.navigation.inertial.calibration.StandardDeviationBodyKinematics;
23 import com.irurueta.navigation.inertial.calibration.StandardDeviationBodyMagneticFluxDensity;
24 import com.irurueta.navigation.inertial.calibration.StandardDeviationTimedBodyKinematics;
25 import com.irurueta.navigation.inertial.calibration.accelerometer.AccelerometerCalibratorMeasurementType;
26 import com.irurueta.navigation.inertial.calibration.accelerometer.AccelerometerNonLinearCalibrator;
27 import com.irurueta.navigation.inertial.calibration.gyroscope.GyroscopeCalibratorMeasurementOrSequenceType;
28 import com.irurueta.navigation.inertial.calibration.gyroscope.GyroscopeNonLinearCalibrator;
29 import com.irurueta.navigation.inertial.calibration.magnetometer.MagnetometerCalibratorMeasurementType;
30 import com.irurueta.navigation.inertial.calibration.magnetometer.MagnetometerNonLinearCalibrator;
31 import com.irurueta.numerical.EvaluationException;
32 import com.irurueta.numerical.InvalidBracketRangeException;
33 import com.irurueta.numerical.NumericalException;
34 import com.irurueta.numerical.SingleDimensionFunctionEvaluatorListener;
35 import com.irurueta.numerical.optimization.BracketedSingleOptimizer;
36 import com.irurueta.numerical.optimization.BrentSingleOptimizer;
37 import com.irurueta.numerical.optimization.OnIterationCompletedListener;
38
39 /**
40 * Optimizes the threshold factor for interval detection of accelerometer and gyroscope
41 * data based on results of calibration.
42 * Only accelerometer calibrators based on unknown orientation are supported (in other terms,
43 * calibrators must be {@link AccelerometerNonLinearCalibrator} and must support
44 * {@link AccelerometerCalibratorMeasurementType#STANDARD_DEVIATION_BODY_KINEMATICS}).
45 * Only gyroscope calibrators based on unknown orientation are supported (in other terms,
46 * calibrators must be {@link GyroscopeNonLinearCalibrator} and must support
47 * {@link GyroscopeCalibratorMeasurementOrSequenceType#BODY_KINEMATICS_SEQUENCE}).
48 * Only magnetometer calibrators based on unknown orientation are supported, in other terms,
49 * calibrators must be {@link MagnetometerNonLinearCalibrator} and must support
50 * {@link MagnetometerCalibratorMeasurementType#STANDARD_DEVIATION_BODY_MAGNETIC_FLUX_DENSITY}.
51 * This implementation uses a {@link BracketedSingleOptimizer} to find the threshold
52 * factor value that minimizes Mean Square Error (MSE) for calibration parameters.
53 */
54 public class BracketedAccelerometerGyroscopeAndMagnetometerIntervalDetectorThresholdFactorOptimizer extends
55 AccelerometerGyroscopeAndMagnetometerIntervalDetectorThresholdFactorOptimizer {
56
57 /**
58 * A bracketed single optimizer to find the threshold factor value that
59 * minimizes the Mean Square Error (MSE) for calibration parameters.
60 */
61 private BracketedSingleOptimizer mseOptimizer;
62
63 /**
64 * Listener for optimizer.
65 */
66 private SingleDimensionFunctionEvaluatorListener optimizerListener;
67
68 /**
69 * Iteration listener for {@link BracketedSingleOptimizer}.
70 */
71 private OnIterationCompletedListener iterationCompletedListener;
72
73 /**
74 * Constructor.
75 */
76 public BracketedAccelerometerGyroscopeAndMagnetometerIntervalDetectorThresholdFactorOptimizer() {
77 super();
78 initializeOptimizerListeners();
79 try {
80 setMseOptimizer(new BrentSingleOptimizer());
81 } catch (final LockedException ignore) {
82 // never happens
83 }
84 }
85
86 /**
87 * Constructor.
88 *
89 * @param dataSource instance in charge of retrieving data for this optimizer.
90 */
91 public BracketedAccelerometerGyroscopeAndMagnetometerIntervalDetectorThresholdFactorOptimizer(
92 final AccelerometerGyroscopeAndMagnetometerIntervalDetectorThresholdFactorOptimizerDataSource dataSource) {
93 super(dataSource);
94 initializeOptimizerListeners();
95 try {
96 setMseOptimizer(new BrentSingleOptimizer());
97 } catch (final LockedException ignore) {
98 // never happens
99 }
100 }
101
102 /**
103 * Constructor.
104 *
105 * @param accelerometerCalibrator an accelerometer calibrator to be used to
106 * optimize its Mean Square Error (MSE).
107 * @param gyroscopeCalibrator a gyroscope calibrator to be used to optimize
108 * its Mean Square Error (MSE).
109 * @param magnetometerCalibrator a magnetometer calibrator to be used to
110 * optimize its Mean Square Error (MSE).
111 * @throws IllegalArgumentException if accelerometer calibrator does not use
112 * {@link StandardDeviationBodyKinematics} measurements,
113 * if gyroscope calibrator does not use
114 * {@link BodyKinematicsSequence} sequences of
115 * {@link StandardDeviationTimedBodyKinematics} or
116 * if magnetometer calibrator does not use
117 * {@link StandardDeviationBodyMagneticFluxDensity} measurements.
118 */
119 public BracketedAccelerometerGyroscopeAndMagnetometerIntervalDetectorThresholdFactorOptimizer(
120 final AccelerometerNonLinearCalibrator accelerometerCalibrator,
121 final GyroscopeNonLinearCalibrator gyroscopeCalibrator,
122 final MagnetometerNonLinearCalibrator magnetometerCalibrator) {
123 super(accelerometerCalibrator, gyroscopeCalibrator, magnetometerCalibrator);
124 initializeOptimizerListeners();
125 try {
126 setMseOptimizer(new BrentSingleOptimizer());
127 } catch (final LockedException ignore) {
128 // never happens
129 }
130 }
131
132 /**
133 * Constructor.
134 *
135 * @param dataSource instance in charge of retrieving data for this optimizer.
136 * @param accelerometerCalibrator an accelerometer calibrator to be used to
137 * optimize its Mean Square Error (MSE).
138 * @param gyroscopeCalibrator a gyroscope calibrator to be used to optimize
139 * its Mean Square Error (MSE).
140 * @param magnetometerCalibrator a magnetometer calibrator to be used to
141 * optimize its Mean Square Error (MSE).
142 * @throws IllegalArgumentException if accelerometer calibrator does not use
143 * {@link StandardDeviationBodyKinematics} measurements,
144 * if gyroscope calibrator does not use
145 * {@link BodyKinematicsSequence} sequences of
146 * {@link StandardDeviationTimedBodyKinematics} or
147 * if magnetometer calibrator does not use
148 * {@link StandardDeviationBodyMagneticFluxDensity} measurements.
149 */
150 public BracketedAccelerometerGyroscopeAndMagnetometerIntervalDetectorThresholdFactorOptimizer(
151 final AccelerometerGyroscopeAndMagnetometerIntervalDetectorThresholdFactorOptimizerDataSource dataSource,
152 final AccelerometerNonLinearCalibrator accelerometerCalibrator,
153 final GyroscopeNonLinearCalibrator gyroscopeCalibrator,
154 final MagnetometerNonLinearCalibrator magnetometerCalibrator) {
155 super(dataSource, accelerometerCalibrator, gyroscopeCalibrator, magnetometerCalibrator);
156 initializeOptimizerListeners();
157 try {
158 setMseOptimizer(new BrentSingleOptimizer());
159 } catch (final LockedException ignore) {
160 // never happens
161 }
162 }
163
164 /**
165 * Constructor.
166 *
167 * @param mseOptimizer optimizer to find the threshold factor value that
168 * minimizes MSE for calibration parameters.
169 */
170 public BracketedAccelerometerGyroscopeAndMagnetometerIntervalDetectorThresholdFactorOptimizer(
171 final BracketedSingleOptimizer mseOptimizer) {
172 super();
173 initializeOptimizerListeners();
174 try {
175 setMseOptimizer(mseOptimizer);
176 } catch (final LockedException ignore) {
177 // never happens
178 }
179 }
180
181 /**
182 * Constructor.
183 *
184 * @param dataSource instance in charge of retrieving data for this optimizer.
185 * @param mseOptimizer optimizer to find the threshold value that minimizes
186 * MSE for calibration parameters.
187 */
188 public BracketedAccelerometerGyroscopeAndMagnetometerIntervalDetectorThresholdFactorOptimizer(
189 final AccelerometerGyroscopeAndMagnetometerIntervalDetectorThresholdFactorOptimizerDataSource dataSource,
190 final BracketedSingleOptimizer mseOptimizer) {
191 super(dataSource);
192 initializeOptimizerListeners();
193 try {
194 setMseOptimizer(mseOptimizer);
195 } catch (final LockedException ignore) {
196 // never happens
197 }
198 }
199
200 /**
201 * Constructor.
202 *
203 * @param accelerometerCalibrator an accelerometer calibrator to be used to
204 * optimize its Mean Square Error (MSE).
205 * @param gyroscopeCalibrator a gyroscope calibrator to be used to optimize
206 * its Mean Square Error (MSE).
207 * @param magnetometerCalibrator a magnetometer calibrator to be used to
208 * optimize its Mean Square Error (MSE).
209 * @param mseOptimizer optimizer to find the threshold value that
210 * minimizes MSE for calibration parameters.
211 * @throws IllegalArgumentException if accelerometer calibrator does not use
212 * {@link StandardDeviationBodyKinematics} measurements,
213 * if gyroscope calibrator does not use
214 * {@link BodyKinematicsSequence} sequences of
215 * {@link StandardDeviationTimedBodyKinematics} or
216 * if magnetometer calibrator does not use
217 * {@link StandardDeviationBodyMagneticFluxDensity} measurements.
218 */
219 public BracketedAccelerometerGyroscopeAndMagnetometerIntervalDetectorThresholdFactorOptimizer(
220 final AccelerometerNonLinearCalibrator accelerometerCalibrator,
221 final GyroscopeNonLinearCalibrator gyroscopeCalibrator,
222 final MagnetometerNonLinearCalibrator magnetometerCalibrator,
223 final BracketedSingleOptimizer mseOptimizer) {
224 super(accelerometerCalibrator, gyroscopeCalibrator, magnetometerCalibrator);
225 initializeOptimizerListeners();
226 try {
227 setMseOptimizer(mseOptimizer);
228 } catch (final LockedException ignore) {
229 // never happens
230 }
231 }
232
233 /**
234 * Constructor.
235 *
236 * @param dataSource instance in charge of retrieving data for this optimizer.
237 * @param accelerometerCalibrator an accelerometer calibrator to be used to
238 * optimize its Mean Square Error (MSE).
239 * @param gyroscopeCalibrator a gyroscope calibrator to be used to optimize
240 * its Mean Square Error (MSE).
241 * @param magnetometerCalibrator a magnetometer calibrator to be used to
242 * optimize its Mean Square Error (MSE).
243 * @param mseOptimizer optimizer to find the threshold value that
244 * minimizes MSE for calibration parameters.
245 * @throws IllegalArgumentException if accelerometer calibrator does not use
246 * {@link StandardDeviationBodyKinematics} measurements,
247 * if gyroscope calibrator does not use
248 * {@link BodyKinematicsSequence} sequences of
249 * {@link StandardDeviationTimedBodyKinematics} or
250 * if magnetometer calibrator does not use
251 * {@link StandardDeviationBodyMagneticFluxDensity} measurements.
252 */
253 public BracketedAccelerometerGyroscopeAndMagnetometerIntervalDetectorThresholdFactorOptimizer(
254 final AccelerometerGyroscopeAndMagnetometerIntervalDetectorThresholdFactorOptimizerDataSource dataSource,
255 final AccelerometerNonLinearCalibrator accelerometerCalibrator,
256 final GyroscopeNonLinearCalibrator gyroscopeCalibrator,
257 final MagnetometerNonLinearCalibrator magnetometerCalibrator,
258 final BracketedSingleOptimizer mseOptimizer) {
259 super(dataSource, accelerometerCalibrator, gyroscopeCalibrator, magnetometerCalibrator);
260 initializeOptimizerListeners();
261 try {
262 setMseOptimizer(mseOptimizer);
263 } catch (final LockedException ignore) {
264 // never happens
265 }
266 }
267
268 /**
269 * Gets the bracketed single optimizer used to find the threshold factor value
270 * that minimizes the Mean Square Error (MSE) for calibration parameters.
271 *
272 * @return optimizer to find the threshold factor value that minimizes the
273 * MSE for calibration parameters.
274 */
275 public BracketedSingleOptimizer getMseOptimizer() {
276 return mseOptimizer;
277 }
278
279 /**
280 * Sets the bracketed single optimizer used to find the threshold factor value
281 * that minimizes the Mean Square Error (MSE) for calibration parameters.
282 *
283 * @param optimizer optimizer to find the threshold factor value that minimizes
284 * the MSE for calibration parameters.
285 * @throws LockedException if optimizer is already running.
286 */
287 public void setMseOptimizer(final BracketedSingleOptimizer optimizer)
288 throws LockedException {
289 if (running) {
290 throw new LockedException();
291 }
292
293 try {
294 if (optimizer != null) {
295 optimizer.setBracket(minThresholdFactor, minThresholdFactor, maxThresholdFactor);
296 optimizer.setListener(optimizerListener);
297 optimizer.setOnIterationCompletedListener(iterationCompletedListener);
298 }
299 mseOptimizer = optimizer;
300 } catch (final com.irurueta.numerical.LockedException e) {
301 throw new LockedException(e);
302 } catch (final InvalidBracketRangeException ignore) {
303 // never happens
304 }
305 }
306
307 /**
308 * Indicates whether this optimizer is ready to start optimization.
309 *
310 * @return true if this optimizer is ready, false otherwise.
311 */
312 @Override
313 public boolean isReady() {
314 return super.isReady() && mseOptimizer != null;
315 }
316
317 /**
318 * Optimizes the threshold factor for a static interval detector or measurement
319 * generator to minimize MSE (Minimum Squared Error) of estimated
320 * calibration parameters.
321 *
322 * @return optimized threshold factor.
323 * @throws NotReadyException if this optimizer is not ready to start optimization.
324 * @throws LockedException if optimizer is already running.
325 * @throws IntervalDetectorThresholdFactorOptimizerException if optimization fails for
326 * some reason.
327 */
328 @Override
329 public double optimize() throws NotReadyException, LockedException,
330 IntervalDetectorThresholdFactorOptimizerException {
331 if (running) {
332 throw new LockedException();
333 }
334
335 if (!isReady()) {
336 throw new NotReadyException();
337 }
338
339 try {
340 running = true;
341
342 initProgress();
343
344 if (listener != null) {
345 listener.onOptimizeStart(this);
346 }
347
348 minMse = Double.MAX_VALUE;
349 mseOptimizer.minimize();
350
351 if (listener != null) {
352 listener.onOptimizeEnd(this);
353 }
354
355 return optimalThresholdFactor;
356 } catch (final NumericalException e) {
357 throw new IntervalDetectorThresholdFactorOptimizerException(e);
358 } finally {
359 running = false;
360 }
361 }
362
363 /**
364 * Initializes optimizer listener.
365 */
366 private void initializeOptimizerListeners() {
367 optimizerListener = point -> {
368 try {
369 return evaluateForThresholdFactor(point);
370 } catch (final NavigationException e) {
371 throw new EvaluationException(e);
372 }
373 };
374
375 iterationCompletedListener = (optimizer, iteration, maxIterations) -> {
376 if (maxIterations == null) {
377 return;
378 }
379
380 progress = (float) iteration / (float) maxIterations;
381 checkAndNotifyProgress();
382 };
383 }
384 }