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.algebra.Matrix;
19 import com.irurueta.algebra.WrongSizeException;
20 import com.irurueta.navigation.inertial.BodyMagneticFluxDensity;
21
22 import java.util.ArrayList;
23 import java.util.Collection;
24
25 /**
26 * Generates body magnetic flux density instances from true body magnetic
27 * flux density values taking into account provided magnetometer errors
28 * for a calibrated magnetometer.
29 */
30 @SuppressWarnings("DuplicatedCode")
31 public class BodyMagneticFluxDensityGenerator {
32
33 /**
34 * Prevents instantiation of utility class.
35 */
36 private BodyMagneticFluxDensityGenerator() {
37 }
38
39 /**
40 * Generates uncalibrated body magnetic flux densities instances for provided
41 * ground-truth body magnetic flux densities.
42 *
43 * @param trueMagneticFluxDensities collection of ground-truth body magnetic
44 * flux densities.
45 * @param magnetometerHardIron magnetometer hard-iron biases. Must
46 * have length 3.
47 * @param magnetometerSoftIron magnetometer soft-iron and
48 * cross-couplings. Must be 3x3.
49 * @return collection of generated uncalibrated magnetic flux densities
50 * for each provided ground-truth one.
51 * @throws IllegalArgumentException if either hard-iron or soft-iron doesn't
52 * have proper size.
53 */
54 public static Collection<BodyMagneticFluxDensity> generate(
55 final Collection<BodyMagneticFluxDensity> trueMagneticFluxDensities, final double[] magnetometerHardIron,
56 final Matrix magnetometerSoftIron) {
57 final var result = new ArrayList<BodyMagneticFluxDensity>();
58 generate(trueMagneticFluxDensities, magnetometerHardIron, magnetometerSoftIron, result);
59 return result;
60 }
61
62 /**
63 * Generates uncalibrated body magnetic flux densities instances for provided
64 * ground-truth body magnetic flux densities.
65 *
66 * @param trueMagneticFluxDensities collection of ground-truth body magnetic
67 * flux densities.
68 * @param magnetometerHardIron magnetometer hard-iron biases. Must
69 * have length 3.
70 * @param magnetometerSoftIron magnetometer soft-iron and
71 * cross-couplings. Must be 3x3.
72 * @param result collection where generated uncalibrated magnetic flux densities
73 * for each provided ground-truth one will be stored.
74 * @throws IllegalArgumentException if either hard-iron or soft-iron doesn't
75 * have proper size.
76 */
77 public static void generate(
78 final Collection<BodyMagneticFluxDensity> trueMagneticFluxDensities, final double[] magnetometerHardIron,
79 final Matrix magnetometerSoftIron, final Collection<BodyMagneticFluxDensity> result) {
80 try {
81 final var mBtrue = new Matrix(BodyMagneticFluxDensity.COMPONENTS, 1);
82 final var identity = Matrix.identity(
83 BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
84 final var tmp33 = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
85 final var tmp31 = new Matrix(BodyMagneticFluxDensity.COMPONENTS, 1);
86
87 for (final var b : trueMagneticFluxDensities) {
88 final var r = new BodyMagneticFluxDensity();
89
90 internalGenerate(b, magnetometerHardIron, magnetometerSoftIron, r, mBtrue, identity, tmp33, tmp31);
91
92 result.add(r);
93 }
94 } catch (final WrongSizeException ignore) {
95 // never happens
96 }
97 }
98
99 /**
100 * Generates an uncalibrated body magnetic flux density instance for provided
101 * ground-truth body magnetic flux density.
102 *
103 * @param trueMagneticFluxDensity a ground-truth body magnetic flux density.
104 * @param magnetometerHardIron magnetometer hard-iron biases. Must
105 * have length 3.
106 * @param magnetometerSoftIron magnetometer soft-iron and
107 * cross-couplings. Must be 3x3.
108 * @return an uncalibrated magnetic flux density.
109 * @throws IllegalArgumentException if either hard-iron or soft-iron doesn't
110 * have proper size.
111 */
112 public static BodyMagneticFluxDensity generate(
113 final BodyMagneticFluxDensity trueMagneticFluxDensity, final double[] magnetometerHardIron,
114 final Matrix magnetometerSoftIron) {
115 final var result = new BodyMagneticFluxDensity();
116 generate(trueMagneticFluxDensity, magnetometerHardIron, magnetometerSoftIron, result);
117 return result;
118 }
119
120 /**
121 * Generates an uncalibrated body magnetic flux density instance for provided
122 * ground-truth body magnetic flux density.
123 *
124 * @param trueMagneticFluxDensity a ground-truth body magnetic flux density.
125 * @param magnetometerHardIron magnetometer hard-iron biases. Must
126 * have length 3.
127 * @param magnetometerSoftIron magnetometer soft-iron and
128 * cross-couplings. Must be 3x3.
129 * @param result instance where uncalibrated magnetic flux
130 * density will be stored.
131 * @throws IllegalArgumentException if either hard-iron or soft-iron doesn't
132 * have proper size.
133 */
134 public static void generate(
135 final BodyMagneticFluxDensity trueMagneticFluxDensity, final double[] magnetometerHardIron,
136 final Matrix magnetometerSoftIron, final BodyMagneticFluxDensity result) {
137 try {
138 final var mBtrue = new Matrix(BodyMagneticFluxDensity.COMPONENTS, 1);
139 final var identity = Matrix.identity(
140 BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
141 final var tmp33 = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
142 final var tmp31 = new Matrix(BodyMagneticFluxDensity.COMPONENTS, 1);
143 internalGenerate(trueMagneticFluxDensity, magnetometerHardIron, magnetometerSoftIron, result, mBtrue,
144 identity, tmp33, tmp31);
145 } catch (final WrongSizeException ignore) {
146 // never happens
147 }
148 }
149
150 /**
151 * Generates an uncalibrated body magnetic flux density instance for provided
152 * ground-truth body magnetic flux density.
153 *
154 * @param trueMagneticFluxDensity a ground-truth body magnetic flux density.
155 * @param magnetometerHardIron magnetometer hard-iron biases. Must
156 * have length 3.
157 * @param magnetometerSoftIron magnetometer soft-iron and
158 * cross-couplings. Must be 3x3.
159 * @param result instance where uncalibrated magnetic flux
160 * density will be stored.
161 * @param mBtrue a 3x1 matrix to be reused to store
162 * ground-truth body magnetic flux
163 * density.
164 * @param identity a 3x3 identity matrix to be reused.
165 * @param tmp33 a 3x3 temporary matrix to be reused.
166 * @param tmp31 a 3x1 temporary matrix to be reused.
167 * @throws WrongSizeException if any of provided matrices has invalid size.
168 */
169 private static void internalGenerate(
170 final BodyMagneticFluxDensity trueMagneticFluxDensity, final double[] magnetometerHardIron,
171 final Matrix magnetometerSoftIron, final BodyMagneticFluxDensity result, final Matrix mBtrue,
172 final Matrix identity, final Matrix tmp33, final Matrix tmp31) throws WrongSizeException {
173 if (magnetometerHardIron.length != BodyMagneticFluxDensity.COMPONENTS) {
174 throw new IllegalArgumentException();
175 }
176 if (magnetometerSoftIron.getRows() != BodyMagneticFluxDensity.COMPONENTS
177 || magnetometerSoftIron.getColumns() != BodyMagneticFluxDensity.COMPONENTS) {
178 throw new IllegalArgumentException();
179 }
180
181 // The magnetometer model is:
182 // mBmeas = bm + (I + Mm) * mBtrue + w
183
184 trueMagneticFluxDensity.asMatrix(mBtrue);
185 tmp33.copyFrom(identity);
186 tmp33.add(magnetometerSoftIron);
187
188 tmp33.multiply(mBtrue, tmp31);
189 for (var i = 0; i < BodyMagneticFluxDensity.COMPONENTS; i++) {
190 tmp31.setElementAtIndex(i, tmp31.getElementAtIndex(i) + magnetometerHardIron[i]);
191 }
192
193 result.setCoordinates(tmp31.getElementAtIndex(0), tmp31.getElementAtIndex(1), tmp31.getElementAtIndex(2));
194 }
195 }