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