1 /*
2 * Copyright (C) 2020 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.accelerometer;
17
18 import com.irurueta.navigation.LockedException;
19 import com.irurueta.navigation.NotReadyException;
20 import com.irurueta.navigation.inertial.calibration.CalibrationException;
21 import com.irurueta.navigation.inertial.calibration.StandardDeviationFrameBodyKinematics;
22 import com.irurueta.numerical.robust.MSACRobustEstimator;
23 import com.irurueta.numerical.robust.MSACRobustEstimatorListener;
24 import com.irurueta.numerical.robust.RobustEstimator;
25 import com.irurueta.numerical.robust.RobustEstimatorException;
26 import com.irurueta.numerical.robust.RobustEstimatorMethod;
27
28 import java.util.List;
29
30 /**
31 * Robustly estimates accelerometer biases, cross couplings and scaling factors
32 * using a MSAC algorithm to discard outliers.
33 * <p>
34 * To use this calibrator at least 4 measurements at different known frames must
35 * be provided. In other words, accelerometer samples must be obtained at 4
36 * different positions, orientations and velocities (although typically velocities are
37 * always zero).
38 * <p>
39 * Measured specific force is assumed to follow the model shown below:
40 * <pre>
41 * fmeas = ba + (I + Ma) * ftrue + w
42 * </pre>
43 * Where:
44 * - fmeas is the measured specific force. This is a 3x1 vector.
45 * - ba is accelerometer bias. Ideally, on a perfect accelerometer, this should be a
46 * 3x1 zero vector.
47 * - I is the 3x3 identity matrix.
48 * - Ma is the 3x3 matrix containing cross-couplings and scaling factors. Ideally, on
49 * a perfect accelerometer, this should be a 3x3 zero matrix.
50 * - ftrue is ground-truth specific force.
51 * - w is measurement noise.
52 */
53 public class MSACRobustKnownFrameAccelerometerCalibrator extends RobustKnownFrameAccelerometerCalibrator {
54
55 /**
56 * Constant defining default threshold to determine whether samples are
57 * inliers or not.
58 */
59 public static final double DEFAULT_THRESHOLD = 1e-2;
60
61 /**
62 * Minimum value that can be set as threshold.
63 * Threshold must be strictly greater than 0.0.
64 */
65 public static final double MIN_THRESHOLD = 0.0;
66
67 /**
68 * Threshold to determine whether samples are inliers or not when
69 * testing possible estimation solutions.
70 */
71 private double threshold = DEFAULT_THRESHOLD;
72
73 /**
74 * Constructor.
75 */
76 public MSACRobustKnownFrameAccelerometerCalibrator() {
77 }
78
79 /**
80 * Constructor.
81 *
82 * @param listener listener to be notified of events such as when estimation
83 * starts, ends or its progress significantly changes.
84 */
85 public MSACRobustKnownFrameAccelerometerCalibrator(
86 final RobustKnownFrameAccelerometerCalibratorListener listener) {
87 super(listener);
88 }
89
90 /**
91 * Constructor.
92 *
93 * @param measurements list of body kinematics measurements with standard
94 * deviations taken at different frames (positions, orientations
95 * and velocities).
96 */
97 public MSACRobustKnownFrameAccelerometerCalibrator(final List<StandardDeviationFrameBodyKinematics> measurements) {
98 super(measurements);
99 }
100
101 /**
102 * Constructor.
103 *
104 * @param measurements list of body kinematics measurements with standard
105 * deviations taken at different frames (positions, orientations
106 * and velocities).
107 * @param listener listener to handle events raised by this calibrator.
108 */
109 public MSACRobustKnownFrameAccelerometerCalibrator(
110 final List<StandardDeviationFrameBodyKinematics> measurements,
111 final RobustKnownFrameAccelerometerCalibratorListener listener) {
112 super(measurements, listener);
113 }
114
115 /**
116 * Constructor.
117 *
118 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
119 * accelerometer and gyroscope.
120 */
121 public MSACRobustKnownFrameAccelerometerCalibrator(final boolean commonAxisUsed) {
122 super(commonAxisUsed);
123 }
124
125 /**
126 * Constructor.
127 *
128 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
129 * accelerometer and gyroscope.
130 * @param listener listener to handle events raised by this calibrator.
131 */
132 public MSACRobustKnownFrameAccelerometerCalibrator(
133 final boolean commonAxisUsed, final RobustKnownFrameAccelerometerCalibratorListener listener) {
134 super(commonAxisUsed, listener);
135 }
136
137 /**
138 * Constructor.
139 *
140 * @param measurements list of body kinematics measurements with standard
141 * deviations taken at different frames (positions, orientations
142 * and velocities).
143 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
144 * accelerometer and gyroscope.
145 */
146 public MSACRobustKnownFrameAccelerometerCalibrator(
147 final List<StandardDeviationFrameBodyKinematics> measurements, final boolean commonAxisUsed) {
148 super(measurements, commonAxisUsed);
149 }
150
151 /**
152 * Constructor.
153 *
154 * @param measurements list of body kinematics measurements with standard
155 * deviations taken at different frames (positions, orientations
156 * and velocities).
157 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
158 * accelerometer and gyroscope.
159 * @param listener listener to handle events raised by this calibrator.
160 */
161 public MSACRobustKnownFrameAccelerometerCalibrator(
162 final List<StandardDeviationFrameBodyKinematics> measurements, final boolean commonAxisUsed,
163 final RobustKnownFrameAccelerometerCalibratorListener listener) {
164 super(measurements, commonAxisUsed, listener);
165 }
166
167 /**
168 * Returns threshold to determine whether samples are inliers or not.
169 *
170 * @return threshold to determine whether samples are inliers or not.
171 */
172 public double getThreshold() {
173 return threshold;
174 }
175
176 /**
177 * Sets threshold to determine whether samples are inliers or not.
178 *
179 * @param threshold threshold to be set.
180 * @throws IllegalArgumentException if provided value is equal or less than
181 * zero.
182 * @throws LockedException if calibrator is currently running.
183 */
184 public void setThreshold(final double threshold) throws LockedException {
185 if (running) {
186 throw new LockedException();
187 }
188 if (threshold <= MIN_THRESHOLD) {
189 throw new IllegalArgumentException();
190 }
191 this.threshold = threshold;
192 }
193
194 /**
195 * Estimates accelerometer calibration parameters containing bias, scale factors
196 * and cross-coupling errors.
197 *
198 * @throws LockedException if calibrator is currently running.
199 * @throws NotReadyException if calibrator is not ready.
200 * @throws CalibrationException if estimation fails for numerical reasons.
201 */
202 @SuppressWarnings("DuplicatedCode")
203 @Override
204 public void calibrate() throws LockedException, NotReadyException, CalibrationException {
205 if (running) {
206 throw new LockedException();
207 }
208 if (!isReady()) {
209 throw new NotReadyException();
210 }
211
212 final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() {
213 @Override
214 public double getThreshold() {
215 return threshold;
216 }
217
218 @Override
219 public int getTotalSamples() {
220 return measurements.size();
221 }
222
223 @Override
224 public int getSubsetSize() {
225 return preliminarySubsetSize;
226 }
227
228 @Override
229 public void estimatePreliminarSolutions(
230 final int[] samplesIndices, final List<PreliminaryResult> solutions) {
231 computePreliminarySolutions(samplesIndices, solutions);
232 }
233
234 @Override
235 public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
236 return computeError(measurements.get(i), currentEstimation);
237 }
238
239 @Override
240 public boolean isReady() {
241 return MSACRobustKnownFrameAccelerometerCalibrator.super.isReady();
242 }
243
244 @Override
245 public void onEstimateStart(final RobustEstimator<PreliminaryResult> estimator) {
246 // no action needed
247 }
248
249 @Override
250 public void onEstimateEnd(final RobustEstimator<PreliminaryResult> estimator) {
251 // no action needed
252 }
253
254 @Override
255 public void onEstimateNextIteration(
256 final RobustEstimator<PreliminaryResult> estimator, final int iteration) {
257 if (listener != null) {
258 listener.onCalibrateNextIteration(
259 MSACRobustKnownFrameAccelerometerCalibrator.this, iteration);
260 }
261 }
262
263 @Override
264 public void onEstimateProgressChange(
265 final RobustEstimator<PreliminaryResult> estimator, final float progress) {
266 if (listener != null) {
267 listener.onCalibrateProgressChange(
268 MSACRobustKnownFrameAccelerometerCalibrator.this, progress);
269 }
270 }
271 });
272
273 try {
274 running = true;
275
276 if (listener != null) {
277 listener.onCalibrateStart(this);
278 }
279
280 inliersData = null;
281 innerEstimator.setConfidence(confidence);
282 innerEstimator.setMaxIterations(maxIterations);
283 innerEstimator.setProgressDelta(progressDelta);
284 final var preliminaryResult = innerEstimator.estimate();
285 inliersData = innerEstimator.getInliersData();
286
287 attemptRefine(preliminaryResult);
288
289 if (listener != null) {
290 listener.onCalibrateEnd(this);
291 }
292
293 } catch (final com.irurueta.numerical.LockedException e) {
294 throw new LockedException(e);
295 } catch (final com.irurueta.numerical.NotReadyException e) {
296 throw new NotReadyException(e);
297 } catch (final RobustEstimatorException e) {
298 throw new CalibrationException(e);
299 } finally {
300 running = false;
301 }
302 }
303
304 /**
305 * Returns method being used for robust estimation.
306 *
307 * @return method being used for robust estimation.
308 */
309 @Override
310 public RobustEstimatorMethod getMethod() {
311 return RobustEstimatorMethod.MSAC;
312 }
313
314 /**
315 * Indicates whether this calibrator requires quality scores for each
316 * measurement or not.
317 *
318 * @return true if quality scores are required, false otherwise.
319 */
320 @Override
321 public boolean isQualityScoresRequired() {
322 return false;
323 }
324 }