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.generators;
17
18 import com.irurueta.navigation.LockedException;
19 import com.irurueta.navigation.inertial.BodyKinematicsAndMagneticFluxDensity;
20 import com.irurueta.navigation.inertial.BodyMagneticFluxDensity;
21 import com.irurueta.navigation.inertial.calibration.StandardDeviationBodyMagneticFluxDensity;
22 import com.irurueta.navigation.inertial.calibration.intervals.TriadStaticIntervalDetector;
23 import com.irurueta.navigation.inertial.calibration.noise.AccumulatedMagneticFluxDensityTriadNoiseEstimator;
24
25 /**
26 * Generates measurements for the calibration of magnetometers by alternating
27 * static and dynamic intervals where device is kept static or moved.
28 * Generated measurements must be used with magnetometer calibrators based
29 * on the knowledge of position on Earth and time instant.
30 * Such calibrators are the following ones:
31 * - {@link com.irurueta.navigation.inertial.calibration.magnetometer.KnownPositionAndInstantMagnetometerCalibrator}
32 * - {@link com.irurueta.navigation.inertial.calibration.magnetometer.KnownHardIronPositionAndInstantMagnetometerCalibrator}
33 * - {@link com.irurueta.navigation.inertial.calibration.magnetometer.RobustKnownPositionAndInstantMagnetometerCalibrator}
34 * and all its implementations.
35 * - {@link com.irurueta.navigation.inertial.calibration.magnetometer.RobustKnownHardIronPositionAndInstantMagnetometerCalibrator}
36 * and all its implementations.
37 */
38 public class MagnetometerMeasurementsGenerator extends
39 MeasurementsGenerator<StandardDeviationBodyMagneticFluxDensity,
40 MagnetometerMeasurementsGenerator, MagnetometerMeasurementsGeneratorListener,
41 BodyKinematicsAndMagneticFluxDensity> {
42
43 /**
44 * Accumulated noise estimator for magnetic flux density measurements.
45 */
46 private final AccumulatedMagneticFluxDensityTriadNoiseEstimator accumulatedEstimator =
47 new AccumulatedMagneticFluxDensityTriadNoiseEstimator();
48
49 /**
50 * Accumulated average x-coordinate of magnetic flux density while body remains in a static interval and
51 * expressed in Teslas (T).
52 */
53 private double avgBx;
54
55 /**
56 * Accumulated average y-coordinate of magnetic flux density while body remains in a static interval and
57 * expressed in Teslas (T).
58 */
59 private double avgBy;
60
61 /**
62 * Accumulated average z-coordinate of magnetic flux density while body remains in a static interval and
63 * expressed in Teslas (T).
64 */
65 private double avgBz;
66
67 /**
68 * Contains standard deviation of magnetic flux density during initialization (i.e. base noise level)
69 * expressed in Teslas (T).
70 */
71 private double stdBNorm;
72
73 /**
74 * Constructor.
75 */
76 public MagnetometerMeasurementsGenerator() {
77 super();
78 }
79
80 /**
81 * Constructor.
82 *
83 * @param listener listener to handle events raised by this generator.
84 */
85 public MagnetometerMeasurementsGenerator(final MagnetometerMeasurementsGeneratorListener listener) {
86 super(listener);
87 }
88
89 /**
90 * Resets this generator.
91 *
92 * @throws LockedException if generator is busy.
93 */
94 @Override
95 public void reset() throws LockedException {
96 super.reset();
97
98 accumulatedEstimator.reset();
99 }
100
101 /**
102 * Post process provided input sample.
103 *
104 * @param sample an input sample.
105 * @throws LockedException if generator is busy.
106 */
107 @Override
108 protected void postProcess(final BodyKinematicsAndMagneticFluxDensity sample) throws LockedException {
109 final var b = sample.getMagneticFluxDensity();
110 if (staticIntervalDetector.getStatus() == TriadStaticIntervalDetector.Status.STATIC_INTERVAL
111 || staticIntervalDetector.getStatus() == TriadStaticIntervalDetector.Status.INITIALIZING) {
112
113 accumulatedEstimator.addTriad(b.getBx(), b.getBy(), b.getBz());
114 avgBx = accumulatedEstimator.getAvgX();
115 avgBy = accumulatedEstimator.getAvgY();
116 avgBz = accumulatedEstimator.getAvgZ();
117
118 } else {
119 accumulatedEstimator.reset();
120 }
121 }
122
123 /**
124 * Gets corresponding acceleration triad from provided input sample.
125 * This method must store the result into {@link #triad}.
126 *
127 * @param sample input sample.
128 */
129 @Override
130 protected void getAccelerationTriadFromInputSample(final BodyKinematicsAndMagneticFluxDensity sample) {
131 sample.getKinematics().getSpecificForceTriad(triad);
132 }
133
134 /**
135 * Handles a static-to-dynamic interval change.
136 *
137 * @param accumulatedAvgX average x-coordinate of measurements during last
138 * static period expressed in meters per squared
139 * second (m/s^2).
140 * @param accumulatedAvgY average y-coordinate of specific force during last
141 * static period expressed in meters per squared
142 * second (m/s^2).
143 * @param accumulatedAvgZ average z-coordinate of specific force during last
144 * static period expressed in meters per squared
145 * second (m/s^2).
146 * @param accumulatedStdX standard deviation of x-coordinate of measurements
147 * during last static period expressed in meters per
148 * squared second (m/s^2).
149 * @param accumulatedStdY standard deviation of y-coordinate of measurements
150 * during last static period expressed in meters per
151 * squared second (m/s^2).
152 * @param accumulatedStdZ standard deviation of z-coordinate of measurements
153 * during last static period expressed in meters per
154 * squared second (m/s^2).
155 */
156 @Override
157 protected void handleStaticToDynamicChange(
158 final double accumulatedAvgX, final double accumulatedAvgY, final double accumulatedAvgZ,
159 final double accumulatedStdX, final double accumulatedStdY, final double accumulatedStdZ) {
160
161 if (!isStaticIntervalSkipped()) {
162
163 final var measurement = new StandardDeviationBodyMagneticFluxDensity(
164 new BodyMagneticFluxDensity(avgBx, avgBy, avgBz), stdBNorm);
165
166 if (listener != null) {
167 listener.onGeneratedMeasurement(this, measurement);
168 }
169 }
170 }
171
172 /**
173 * Handles a dynamic-to-static interval change.
174 */
175 @Override
176 protected void handleDynamicToStaticChange() {
177 // no action needed.
178 }
179
180 /**
181 * Handles an initialization completion.
182 */
183 @Override
184 protected void handleInitializationCompleted() {
185 stdBNorm = accumulatedEstimator.getStandardDeviationNorm();
186 }
187
188 /**
189 * Handles an error during initialization.
190 */
191 @Override
192 protected void handleInitializationFailed() {
193 try {
194 accumulatedEstimator.reset();
195 } catch (final LockedException ignore) {
196 // no action needed
197 }
198 }
199 }