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;
17
18 import com.irurueta.navigation.NotReadyException;
19
20 /**
21 * Utility class to create {@link IMUErrors} by combining different sources of estimated data.
22 * Sources of data can be any accelerometer calibrator implementing {@link AccelerometerCalibrationSource},
23 * any gyroscope calibrator implementing {@link GyroscopeCalibrationSource}, or any measurement
24 * generator, static interval detector or noise estimator implementing {@link AccelerometerNoiseRootPsdSource}
25 * or {@link GyroscopeNoiseRootPsdSource}.
26 */
27 public class IMUErrorsCreator {
28
29 /**
30 * A source of estimated accelerometer calibration parameters.
31 */
32 private AccelerometerCalibrationSource accelerometerCalibrationSource;
33
34 /**
35 * A source of estimated gyroscope calibration parameters.
36 */
37 private GyroscopeCalibrationSource gyroscopeCalibrationSource;
38
39 /**
40 * A source of estimated accelerometer noise root PSD.
41 */
42 private AccelerometerNoiseRootPsdSource accelerometerNoiseRootPsdSource;
43
44 /**
45 * A source of estimated gyroscope noise root PSD.
46 */
47 private GyroscopeNoiseRootPsdSource gyroscopeNoiseRootPsdSource;
48
49 /**
50 * Accelerometer quantization level expressed in meters per squared second (m/s^2).
51 * By default, it is zero when no quantization is assumed.
52 */
53 private double accelerometerQuantizationLevel;
54
55 /**
56 * Gyro quantization level expressed in radians per second (rad/s).
57 * By default, it is zero when no quantization is assumed.
58 */
59 private double gyroQuantizationLevel;
60
61 /**
62 * Constructor.
63 */
64 public IMUErrorsCreator() {
65 }
66
67 /**
68 * Constructor.
69 *
70 * @param accelerometerCalibrationSource A source of estimated accelerometer calibration parameters.
71 * @param gyroscopeCalibrationSource A source of estimated gyroscope calibration parameters.
72 * @param accelerometerNoiseRootPsdSource A source of estimated accelerometer noise root PSD.
73 * @param gyroscopeNoiseRootPsdSource A source of estimated gyroscope noise root PSD.
74 */
75 public IMUErrorsCreator(
76 final AccelerometerCalibrationSource accelerometerCalibrationSource,
77 final GyroscopeCalibrationSource gyroscopeCalibrationSource,
78 final AccelerometerNoiseRootPsdSource accelerometerNoiseRootPsdSource,
79 final GyroscopeNoiseRootPsdSource gyroscopeNoiseRootPsdSource) {
80 this.accelerometerCalibrationSource = accelerometerCalibrationSource;
81 this.gyroscopeCalibrationSource = gyroscopeCalibrationSource;
82 this.accelerometerNoiseRootPsdSource = accelerometerNoiseRootPsdSource;
83 this.gyroscopeNoiseRootPsdSource = gyroscopeNoiseRootPsdSource;
84 }
85
86 /**
87 * Constructor.
88 *
89 * @param accelerometerCalibrationSource A source of estimated accelerometer calibration parameters.
90 * @param gyroscopeCalibrationSource A source of estimated gyroscope calibration parameters.
91 * @param accelerometerNoiseRootPsdSource A source of estimated accelerometer noise root PSD.
92 * @param gyroscopeNoiseRootPsdSource A source of estimated gyroscope noise root PSD.
93 * @param accelerometerQuantizationLevel Accelerometer quantization level expressed in meters per squared
94 * second (m/s^2).
95 * @param gyroQuantizationLevel Gyro quantization level expressed in radians per second (rad/s).
96 */
97 public IMUErrorsCreator(
98 final AccelerometerCalibrationSource accelerometerCalibrationSource,
99 final GyroscopeCalibrationSource gyroscopeCalibrationSource,
100 final AccelerometerNoiseRootPsdSource accelerometerNoiseRootPsdSource,
101 final GyroscopeNoiseRootPsdSource gyroscopeNoiseRootPsdSource,
102 final double accelerometerQuantizationLevel,
103 final double gyroQuantizationLevel) {
104 this(accelerometerCalibrationSource, gyroscopeCalibrationSource, accelerometerNoiseRootPsdSource,
105 gyroscopeNoiseRootPsdSource);
106 this.accelerometerQuantizationLevel = accelerometerQuantizationLevel;
107 this.gyroQuantizationLevel = gyroQuantizationLevel;
108 }
109
110 /**
111 * Gets source of estimated accelerometer calibration parameters.
112 *
113 * @return source of estimated accelerometer calibration parameters.
114 */
115 public AccelerometerCalibrationSource getAccelerometerCalibrationSource() {
116 return accelerometerCalibrationSource;
117 }
118
119 /**
120 * Sets source of estimated accelerometer calibration parameters.
121 *
122 * @param accelerometerCalibrationSource source of estimated accelerometer
123 * parameters.
124 */
125 public void setAccelerometerCalibrationSource(final AccelerometerCalibrationSource accelerometerCalibrationSource) {
126 this.accelerometerCalibrationSource = accelerometerCalibrationSource;
127 }
128
129 /**
130 * Gets source of estimated gyroscope calibration parameters.
131 *
132 * @return source of estimated gyroscope calibration parameters.
133 */
134 public GyroscopeCalibrationSource getGyroscopeCalibrationSource() {
135 return gyroscopeCalibrationSource;
136 }
137
138 /**
139 * Sets source of estimated gyroscope calibration parameters.
140 *
141 * @param gyroscopeCalibrationSource source of estimated gyroscope calibration
142 * parameters.
143 */
144 public void setGyroscopeCalibrationSource(final GyroscopeCalibrationSource gyroscopeCalibrationSource) {
145 this.gyroscopeCalibrationSource = gyroscopeCalibrationSource;
146 }
147
148 /**
149 * Gets source of estimated accelerometer noise root PSD.
150 *
151 * @return source of estimated accelerometer noise root PSD.
152 */
153 public AccelerometerNoiseRootPsdSource getAccelerometerNoiseRootPsdSource() {
154 return accelerometerNoiseRootPsdSource;
155 }
156
157 /**
158 * Sets source of estimated accelerometer noise root PSD.
159 *
160 * @param accelerometerNoiseRootPsdSource source of estimated accelerometer
161 * noise root PSD.
162 */
163 public void setAccelerometerNoiseRootPsdSource(
164 final AccelerometerNoiseRootPsdSource accelerometerNoiseRootPsdSource) {
165 this.accelerometerNoiseRootPsdSource = accelerometerNoiseRootPsdSource;
166 }
167
168 /**
169 * Gets source of estimated gyroscope noise root PSD.
170 *
171 * @return source of estimated gyroscope noise root PSD.
172 */
173 public GyroscopeNoiseRootPsdSource getGyroscopeNoiseRootPsdSource() {
174 return gyroscopeNoiseRootPsdSource;
175 }
176
177 /**
178 * Sets source of estimated gyroscope noise root PSD.
179 *
180 * @param gyroscopeNoiseRootPsdSource source of estimated gyroscope noise
181 * root PSD.
182 */
183 public void sstGyroscopeNoiseRootPsdSource(final GyroscopeNoiseRootPsdSource gyroscopeNoiseRootPsdSource) {
184 this.gyroscopeNoiseRootPsdSource = gyroscopeNoiseRootPsdSource;
185 }
186
187 /**
188 * Gets accelerometer quantization level expressed in meters per squared second
189 * (m/s^2).
190 * By default it is zero when no quantization is assumed.
191 *
192 * @return accelerometer quantization level.
193 */
194 public double getAccelerometerQuantizationLevel() {
195 return accelerometerQuantizationLevel;
196 }
197
198 /**
199 * Sets accelerometer quantization level expressed in meters per squared second
200 * (m/s^2).
201 *
202 * @param accelerometerQuantizationLevel accelerometer quantization level.
203 */
204 public void setAccelerometerQuantizationLevel(final double accelerometerQuantizationLevel) {
205 this.accelerometerQuantizationLevel = accelerometerQuantizationLevel;
206 }
207
208 /**
209 * Gets gyroscope quantization level expressed in radians per second (rad/s).
210 * By default it is zero when no quantization is assumed.
211 *
212 * @return gyroscope quantization level.
213 */
214 public double getGyroQuantizationLevel() {
215 return gyroQuantizationLevel;
216 }
217
218 /**
219 * Sets gyroscope quantization level expressed in radians per second (rad/s).
220 *
221 * @param gyroQuantizationLevel gyroscope quantization level.
222 */
223 public void setGyroQuantizationLevel(final double gyroQuantizationLevel) {
224 this.gyroQuantizationLevel = gyroQuantizationLevel;
225 }
226
227 /**
228 * Indicates whether all sources have been provided in order to be able
229 * to create an {@link IMUErrors} instance.
230 *
231 * @return true if creator is ready, false otherwise.
232 */
233 public boolean isReady() {
234 return accelerometerNoiseRootPsdSource != null && gyroscopeNoiseRootPsdSource != null
235 && accelerometerCalibrationSource != null
236 && accelerometerCalibrationSource.getEstimatedBiases() != null
237 && accelerometerCalibrationSource.getEstimatedMa() != null
238 && gyroscopeCalibrationSource != null
239 && gyroscopeCalibrationSource.getEstimatedBiases() != null
240 && gyroscopeCalibrationSource.getEstimatedMg() != null
241 && gyroscopeCalibrationSource.getEstimatedGg() != null;
242 }
243
244 /**
245 * Creates an {@link IMUErrors} instance containing estimated IMU
246 * (accelerometer + gyroscope) parameters.
247 *
248 * @return instance containing estimated IMU parameters.
249 * @throws NotReadyException if creator is not ready.
250 */
251 public IMUErrors create() throws NotReadyException {
252 if (!isReady()) {
253 throw new NotReadyException();
254 }
255
256 final var ba = accelerometerCalibrationSource.getEstimatedBiases();
257 final var ma = accelerometerCalibrationSource.getEstimatedMa();
258
259 final var bg = gyroscopeCalibrationSource.getEstimatedBiases();
260 final var mg = gyroscopeCalibrationSource.getEstimatedMg();
261 final var gg = gyroscopeCalibrationSource.getEstimatedGg();
262
263 final var accelerometerNoiseRootPsd = accelerometerNoiseRootPsdSource.getAccelerometerBaseNoiseLevelRootPsd();
264 final var gyroscopeNoiseRootPsd = gyroscopeNoiseRootPsdSource.getGyroscopeBaseNoiseLevelRootPsd();
265
266 return new IMUErrors(ba, bg, ma, mg, gg, accelerometerNoiseRootPsd, gyroscopeNoiseRootPsd,
267 accelerometerQuantizationLevel, gyroQuantizationLevel);
268 }
269 }