1 /*
2 * Copyright (C) 2023 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.gyroscope;
17
18 import com.irurueta.algebra.AlgebraException;
19 import com.irurueta.algebra.FrobeniusNormComputer;
20 import com.irurueta.algebra.Matrix;
21 import com.irurueta.geometry.Quaternion;
22 import com.irurueta.geometry.Rotation3D;
23 import com.irurueta.geometry.RotationException;
24
25 /**
26 * Computes an integration step of a quaternion using Suh's method.
27 * More information available here:
28 * Young Soo Suh. "Orientation estimation using a quaternion-based indirect Kalman filter with adaptive estimation of
29 * external acceleration". 2010.
30 */
31 public class SuhQuaternionStepIntegrator extends QuaternionStepIntegrator {
32
33 /**
34 * Precomputed 3/4 factor
35 */
36 private static final double THREE_FOURTHS = 3.0 / 4.0;
37
38 /**
39 * Angular speed at initial timestamp t0 to be reused.
40 */
41 private Matrix omega0;
42
43 /**
44 * Angular speed at end timestamp t1 to be reused.
45 */
46 private Matrix omega1;
47
48 /**
49 * Initial attitude to be reused.
50 */
51 private Matrix quat;
52
53 /**
54 * Skew matrix of omega0 to be reused.
55 */
56 private Matrix omegaSkew0;
57
58 /**
59 * Skew matrix of omega1 to be reused.
60 */
61 private Matrix omegaSkew1;
62
63 /**
64 * Constant matrix to be reused.
65 */
66 private Matrix constant;
67
68 /**
69 * Temporary matrix to be reused.
70 */
71 private Matrix omegaSkew10;
72
73 /**
74 * Identity matrix to be reused.
75 */
76 private Matrix identity;
77
78 /**
79 * Temporary matrix to be reused.
80 */
81 private Matrix omegaSkew1A;
82
83 /**
84 * Temporary matrix to be reused.
85 */
86 private Matrix omegaSkew0A;
87
88 /**
89 * Temporary matrix to be reused.
90 */
91 private Matrix omegaSkew1B;
92
93 /**
94 * Temporary matrix to be reused.
95 */
96 private Matrix tmp;
97
98 /**
99 * Instance where result of integration is stored in matrix form being reused.
100 */
101 private Matrix quatResult;
102
103 /**
104 * Constructor.
105 * Initializes matrices being reused.
106 */
107 public SuhQuaternionStepIntegrator() {
108 try {
109 omega0 = new Matrix(Rotation3D.INHOM_COORDS, 1);
110 omega1 = new Matrix(Rotation3D.INHOM_COORDS, 1);
111 quat = new Matrix(Quaternion.N_PARAMS, 1);
112 omegaSkew0 = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
113 omegaSkew1 = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
114 constant = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
115 omegaSkew10 = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
116 identity = Matrix.identity(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
117 omegaSkew1A = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
118 omegaSkew0A = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
119 omegaSkew1B = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
120 tmp = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
121 quatResult = new Matrix(Quaternion.N_PARAMS, 1);
122 } catch (final AlgebraException ignore) {
123 // never happens
124 }
125 }
126
127 /**
128 * Gets type of this integrator.
129 *
130 * @return indicates type of this integrator.
131 */
132 @Override
133 public QuaternionStepIntegratorType getType() {
134 return QuaternionStepIntegratorType.SUH;
135 }
136
137 /**
138 * Performs Suh's integration step.
139 * More information available here:
140 * Young Soo Suh. "Orientation estimation using a quaternion-based indirect Kalman filter with adaptive estimation
141 * of external acceleration". 2010.
142 *
143 * @param initialAttitude initial attitude.
144 * @param initialWx initial x-coordinate rotation velocity at initial timestamp expressed
145 * in radians per second (rad/s).
146 * @param initialWy initial y-coordinate rotation velocity at initial timestamp expressed
147 * in radians per second (rad/s).
148 * @param initialWz initial z-coordinate rotation velocity at initial timestamp expressed
149 * in radians per second (rad/s).*
150 * @param currentWx end x-coordinate rotation velocity at current timestamp expressed in
151 * radians per second (rad/s).
152 * @param currentWy end y-coordinate rotation velocity at current timestamp expressed in
153 * radians per second (rad/s).
154 * @param currentWz end z-coordinate rotation velocity at current timestamp expressed in
155 * radians per second (rad/s).
156 * @param dt time step expressed in seconds.
157 * @param result instance where result of integration will be stored.
158 * @throws RotationException if a numerical error occurs.
159 */
160 @Override
161 public void integrate(
162 final Quaternion initialAttitude,
163 final double initialWx, final double initialWy, final double initialWz,
164 final double currentWx, final double currentWy, final double currentWz,
165 final double dt, final Quaternion result) throws RotationException {
166 integrationStep(initialAttitude, initialWx, initialWy, initialWz,
167 currentWx, currentWy, currentWz, dt, result, omega0, omega1, quat, omegaSkew0,
168 omegaSkew1, constant, omegaSkew10, identity, omegaSkew1A, omegaSkew0A,
169 omegaSkew1B, tmp, quatResult);
170 }
171
172 /**
173 * Performs Suh's integration step.
174 * More information available here:
175 * Young Soo Suh. "Orientation estimation using a quaternion-based indirect Kalman filter with adaptive estimation
176 * of external acceleration". 2010.
177 *
178 * @param initialAttitude initial attitude.
179 * @param initialWx initial x-coordinate rotation velocity at initial timestamp expressed
180 * in radians per second (rad/s).
181 * @param initialWy initial y-coordinate rotation velocity at initial timestamp expressed
182 * in radians per second (rad/s).
183 * @param initialWz initial z-coordinate rotation velocity at initial timestamp expressed
184 * in radians per second (rad/s).
185 * @param currentWx end x-coordinate rotation velocity at end timestamp expressed in
186 * radians per second (rad/s).
187 * @param currentWy end y-coordinate rotation velocity at end timestamp expressed in
188 * radians per second (rad/s).
189 * @param currentWz end z-coordinate rotation velocity at end timestamp expressed in
190 * radians per second (rad/s).
191 * @param dt time step expressed in seconds (t1 - t0).
192 * @param result instance where result of integration will be stored.
193 * @throws RotationException if a numerical error occurs.
194 */
195 public static void integrationStep(
196 final Quaternion initialAttitude,
197 final double initialWx, final double initialWy, final double initialWz,
198 final double currentWx, final double currentWy, final double currentWz,
199 final double dt, final Quaternion result) throws RotationException {
200 try {
201 final var omega0 = new Matrix(Rotation3D.INHOM_COORDS, 1);
202 final var omega1 = new Matrix(Rotation3D.INHOM_COORDS, 1);
203 final var quat = new Matrix(Quaternion.N_PARAMS, 1);
204 final var omegaSkew0 = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
205 final var omegaSkew1 = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
206 final var constant = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
207 final var omegaSkew10 = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
208 final var identity = Matrix.identity(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
209 final var omegaSkew1A = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
210 final var omegaSkew0A = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
211 final var omegaSkew1B = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
212 final var tmp = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
213 final var quatResult = new Matrix(Quaternion.N_PARAMS, 1);
214 integrationStep(initialAttitude, initialWx, initialWy, initialWz,
215 currentWx, currentWy, currentWz, dt, result, omega0, omega1, quat, omegaSkew0,
216 omegaSkew1, constant, omegaSkew10, identity, omegaSkew1A, omegaSkew0A,
217 omegaSkew1B, tmp, quatResult);
218 } catch (final AlgebraException ignore) {
219 // never happens
220 }
221 }
222
223 /**
224 * Internal method computing an integration step using Suh's algorithm.
225 * This method is used internally so that reusable instances can be provided as parameters.
226 *
227 * @param initialAttitude initial attitude.
228 * @param initialWx initial x-coordinate rotation velocity at initial timestamp expressed
229 * in radians per second (rad/s).
230 * @param initialWy initial y-coordinate rotation velocity at initial timestamp expressed
231 * in radians per second (rad/s).
232 * @param initialWz initial z-coordinate rotation velocity at initial timestamp expressed
233 * in radians per second (rad/s).
234 * @param currentWx end x-coordinate rotation velocity at end timestamp expressed in
235 * radians per second (rad/s).
236 * @param currentWy end y-coordinate rotation velocity at end timestamp expressed in
237 * radians per second (rad/s).
238 * @param currentWz end z-coordinate rotation velocity at end timestamp expressed in
239 * radians per second (rad/s).
240 * @param dt time step expressed in seconds (t1 - t0).
241 * @param result instance where result of integration will be stored.
242 * @param omega0 angular speed at initial timestamp t0 to be reused. Must be 3x1.
243 * @param omega1 angular speed at end timestamp t1 to be reused. Must be 3x1.
244 * @param quat initial attitude to be reused. Must be 4x1.
245 * @param omegaSkew0 skew matrix of omega0 to be reused. Must be 4x4.
246 * @param omegaSkew1 skew matrix of omega1 to be reused. Must be 4x4.
247 * @param constant constant matrix to be reused. Must be 4x4.
248 * @param omegaSkew10 temporary matrix to be reused. Must be 4x4.
249 * @param identity identity matrix to be reused. Must be 4x4.
250 * @param omegaSkew1A temporary matrix to be reused. Must be 4x4.
251 * @param omegaSkew0A temporary matrix to be reused. Must be 4x4.
252 * @param omegaSkew1B temporary matrix to be reused. Must be 4x4.
253 * @param tmp temporary matrix to be reused. Must be 4x4.
254 * @param quatResult instance where result of integration is stored in matrix form being
255 * reused. Must be 4x1.
256 * @throws RotationException if a numerical error occurs.
257 */
258 private static void integrationStep(
259 final Quaternion initialAttitude,
260 final double initialWx, final double initialWy, final double initialWz,
261 final double currentWx, final double currentWy, final double currentWz,
262 final double dt, final Quaternion result, final Matrix omega0, final Matrix omega1,
263 final Matrix quat, final Matrix omegaSkew0, final Matrix omegaSkew1,
264 final Matrix constant, final Matrix omegaSkew10, final Matrix identity,
265 final Matrix omegaSkew1A, final Matrix omegaSkew0A, final Matrix omegaSkew1B,
266 final Matrix tmp, final Matrix quatResult) throws RotationException {
267 try {
268 // normalize and copy initial attitude into matrix form
269 initialAttitude.normalize();
270 initialAttitude.values(quat.getBuffer());
271
272 // angular speed at initial timestamp t0
273 copyAngularSpeedToMatrix(initialWx, initialWy, initialWz, omega0);
274
275 // angular speed at end timestamp t1
276 copyAngularSpeedToMatrix(currentWx, currentWy, currentWz, omega1);
277
278 final var norm1 = FrobeniusNormComputer.norm(omega1);
279 final var sqrNorm1 = norm1 * norm1;
280
281 computeOmegaSkew(omega0, omegaSkew0);
282 computeOmegaSkew(omega1, omegaSkew1);
283
284 final var dt2 = dt * dt;
285 final var dt3 = dt * dt2;
286
287 constant.initialize(sqrNorm1 * dt2 / 6.0);
288
289 omegaSkew1.multiply(omegaSkew0, omegaSkew10);
290 omegaSkew10.multiplyByScalar(dt2 / 24.0);
291
292 omegaSkew1A.copyFrom(omegaSkew1);
293 omegaSkew1A.multiplyByScalar(THREE_FOURTHS * dt);
294
295 omegaSkew0A.copyFrom(omegaSkew0);
296 omegaSkew0A.multiplyByScalar(dt / 4.0);
297
298 omegaSkew1B.copyFrom(omegaSkew1);
299 omegaSkew1B.multiplyByScalar(sqrNorm1 * dt3 / 48.0);
300
301 tmp.copyFrom(identity);
302 tmp.add(omegaSkew1A);
303 tmp.subtract(omegaSkew0A);
304 tmp.subtract(constant);
305 tmp.subtract(omegaSkew10);
306 tmp.subtract(omegaSkew1B);
307
308 tmp.multiply(quat, quatResult);
309
310 result.setValues(quatResult.getBuffer());
311 result.normalize();
312 } catch (final AlgebraException e) {
313 throw new RotationException(e);
314 }
315 }
316 }