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.NotReadyException;
20 import com.irurueta.navigation.inertial.calibration.BodyKinematicsSequence;
21 import com.irurueta.navigation.inertial.calibration.StandardDeviationBodyKinematics;
22 import com.irurueta.navigation.inertial.calibration.StandardDeviationBodyMagneticFluxDensity;
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.navigation.inertial.calibration.magnetometer.MagnetometerCalibratorMeasurementType;
29 import com.irurueta.navigation.inertial.calibration.magnetometer.MagnetometerNonLinearCalibrator;
30
31 /**
32 * Optimizes the threshold factor for interval detection of accelerometer and gyroscope
33 * data based on results of calibration.
34 * Only accelerometer calibrators based on unknown orientation are supported (in other terms,
35 * calibrators must be {@link AccelerometerNonLinearCalibrator} and must support
36 * {@link AccelerometerCalibratorMeasurementType#STANDARD_DEVIATION_BODY_KINEMATICS}).
37 * Only gyroscope calibrators based on unknown orientation are supported (in other terms,
38 * calibrators must be {@link GyroscopeNonLinearCalibrator} and must support
39 * {@link GyroscopeCalibratorMeasurementOrSequenceType#BODY_KINEMATICS_SEQUENCE}).
40 * Only magnetometer calibrators based on unknown orientation are supported, in other terms,
41 * calibrators must be {@link MagnetometerNonLinearCalibrator} and must support
42 * {@link MagnetometerCalibratorMeasurementType#STANDARD_DEVIATION_BODY_MAGNETIC_FLUX_DENSITY}.
43 * This implementation makes an exhaustive search between minimum and maximum
44 * threshold factor values to find the threshold value that produces the
45 * minimum Mean Square Error (MSE) for calibration parameters.
46 */
47 public class ExhaustiveAccelerometerGyroscopeAndMagnetometerIntervalDetectorThresholdFactorOptimizer extends
48 AccelerometerGyroscopeAndMagnetometerIntervalDetectorThresholdFactorOptimizer {
49
50 /**
51 * Default step value to make exhaustive search of threshold factor values.
52 */
53 public static final double DEFAULT_STEP = 1.0;
54
55 /**
56 * Step to make exhaustive search of threshold factor values between
57 * {@link #getMaxThresholdFactor()} and {@link #getMaxThresholdFactor()}.
58 */
59 private double thresholdFactorStep = DEFAULT_STEP;
60
61 /**
62 * Constructor.
63 */
64 public ExhaustiveAccelerometerGyroscopeAndMagnetometerIntervalDetectorThresholdFactorOptimizer() {
65 super();
66 }
67
68 /**
69 * Constructor.
70 *
71 * @param dataSource instance in charge of retrieving data for this optimizer.
72 */
73 public ExhaustiveAccelerometerGyroscopeAndMagnetometerIntervalDetectorThresholdFactorOptimizer(
74 final AccelerometerGyroscopeAndMagnetometerIntervalDetectorThresholdFactorOptimizerDataSource dataSource) {
75 super(dataSource);
76 }
77
78 /**
79 * Constructor.
80 *
81 * @param accelerometerCalibrator an accelerometer calibrator to be used to
82 * optimize its Mean Square Error (MSE).
83 * @param gyroscopeCalibrator a gyroscope calibrator to be used to optimize
84 * its Mean Square Error (MSE).
85 * @param magnetometerCalibrator a magnetometer calibrator to be used to
86 * optimize its Mean Square Error (MSE).
87 * @throws IllegalArgumentException if accelerometer calibrator does not use
88 * {@link StandardDeviationBodyKinematics} measurements,
89 * if gyroscope calibrator does not use
90 * {@link BodyKinematicsSequence} sequences of
91 * {@link StandardDeviationTimedBodyKinematics} or
92 * if magnetometer calibrator does not use
93 * {@link StandardDeviationBodyMagneticFluxDensity} measurements.
94 */
95 public ExhaustiveAccelerometerGyroscopeAndMagnetometerIntervalDetectorThresholdFactorOptimizer(
96 final AccelerometerNonLinearCalibrator accelerometerCalibrator,
97 final GyroscopeNonLinearCalibrator gyroscopeCalibrator,
98 final MagnetometerNonLinearCalibrator magnetometerCalibrator) {
99 super(accelerometerCalibrator, gyroscopeCalibrator, magnetometerCalibrator);
100 }
101
102 /**
103 * Constructor.
104 *
105 * @param dataSource instance in charge of retrieving data for this optimizer.
106 * @param accelerometerCalibrator an accelerometer calibrator to be used to
107 * optimize its Mean Square Error (MSE).
108 * @param gyroscopeCalibrator a gyroscope calibrator to be used to optimize
109 * its Mean Square Error (MSE).
110 * @param magnetometerCalibrator a magnetometer calibrator to be used to
111 * optimize its Mean Square Error (MSE).
112 * @throws IllegalArgumentException if accelerometer calibrator does not use
113 * {@link StandardDeviationBodyKinematics} measurements,
114 * if gyroscope calibrator does not use
115 * {@link BodyKinematicsSequence} sequences of
116 * {@link StandardDeviationTimedBodyKinematics} or
117 * if magnetometer calibrator does not use
118 * {@link StandardDeviationBodyMagneticFluxDensity} measurements.
119 */
120 public ExhaustiveAccelerometerGyroscopeAndMagnetometerIntervalDetectorThresholdFactorOptimizer(
121 final AccelerometerGyroscopeAndMagnetometerIntervalDetectorThresholdFactorOptimizerDataSource dataSource,
122 final AccelerometerNonLinearCalibrator accelerometerCalibrator,
123 final GyroscopeNonLinearCalibrator gyroscopeCalibrator,
124 final MagnetometerNonLinearCalibrator magnetometerCalibrator) {
125 super(dataSource, accelerometerCalibrator, gyroscopeCalibrator, magnetometerCalibrator);
126 }
127
128 /**
129 * Gets the step to make exhaustive search of threshold values between
130 * {@link #getMaxThresholdFactor()} and {@link #getMaxThresholdFactor()}.
131 *
132 * @return step to make exhaustive search of threshold values.
133 */
134 public double getThresholdFactorStep() {
135 return thresholdFactorStep;
136 }
137
138 /**
139 * Sets step to make exhaustive search of threshold values between
140 * {@link #getMaxThresholdFactor()} and {@link #getMaxThresholdFactor()}.
141 *
142 * @param thresholdStep step to make exhaustive search of threshold values.
143 * @throws LockedException if optimizer is already running.
144 * @throws IllegalArgumentException if provided value is zero or negative.
145 */
146 public void setThresholdFactorStep(final double thresholdStep) throws LockedException {
147 if (running) {
148 throw new LockedException();
149 }
150 if (thresholdStep <= 0.0) {
151 throw new IllegalArgumentException();
152 }
153
154 thresholdFactorStep = thresholdStep;
155 }
156
157 /**
158 * Optimizes the threshold factor for a static interval detector or measurement
159 * generator to minimize MSE (Minimum Squared Error) of estimated
160 * calibration parameters.
161 *
162 * @return optimized threshold factor.
163 * @throws NotReadyException if this optimizer is not ready to start optimization.
164 * @throws LockedException if optimizer is already running.
165 * @throws IntervalDetectorThresholdFactorOptimizerException if optimization fails for
166 * some reason.
167 */
168 @Override
169 public double optimize() throws NotReadyException, LockedException,
170 IntervalDetectorThresholdFactorOptimizerException {
171 if (running) {
172 throw new LockedException();
173 }
174
175 if (!isReady()) {
176 throw new NotReadyException();
177 }
178
179 var hasResult = false;
180 minMse = Double.MAX_VALUE;
181 try {
182 running = true;
183
184 initProgress();
185 final var progressStep = (float) (thresholdFactorStep
186 / (thresholdFactorStep + maxThresholdFactor - minThresholdFactor));
187
188 if (listener != null) {
189 listener.onOptimizeStart(this);
190 }
191
192 for (var thresholdFactor = minThresholdFactor;
193 thresholdFactor <= maxThresholdFactor;
194 thresholdFactor += thresholdFactorStep) {
195 try {
196 evaluateForThresholdFactor(thresholdFactor);
197 hasResult = true;
198 } catch (final Exception ignore) {
199 // when an error occurs, skip to the next iteration
200 }
201
202 progress += progressStep;
203 checkAndNotifyProgress();
204 }
205
206 if (!hasResult) {
207 throw new IntervalDetectorThresholdFactorOptimizerException();
208 }
209
210 if (listener != null) {
211 listener.onOptimizeEnd(this);
212 }
213
214 } finally {
215 running = false;
216 }
217
218 return optimalThresholdFactor;
219 }
220 }