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