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