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