1 /*
2 * Copyright (C) 2022 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.Matrix;
20 import com.irurueta.geometry.Quaternion;
21 import com.irurueta.geometry.Rotation3D;
22 import com.irurueta.geometry.RotationException;
23
24 /**
25 * Computes an integration step of a quaternion using Euler's method.
26 * More information available here:
27 * <a href="https://en.wikipedia.org/wiki/Euler_method">https://en.wikipedia.org/wiki/Euler_method</a>
28 */
29 public class EulerQuaternionStepIntegrator extends QuaternionStepIntegrator {
30
31 /**
32 * Angular speed at initial timestamp t0 to be reused.
33 */
34 private Matrix omega0;
35
36 /**
37 * Initial attitude to be reused.
38 */
39 private Matrix quat;
40
41 /**
42 * Instance where result of integration is stored in matrix form being reused.
43 */
44 private Matrix quatResult;
45
46 /**
47 * Slope of quaternion derivative at initial timestamp t0 to be reused.
48 */
49 private Matrix k1;
50
51 /**
52 * Skew antisymmetric matrix used for quaternion time derivative computation to be reused.
53 */
54 private Matrix omegaSkew;
55
56 /**
57 * Constructor.
58 * Initializes matrices being reused.
59 */
60 public EulerQuaternionStepIntegrator() {
61 try {
62 omega0 = new Matrix(Rotation3D.INHOM_COORDS, 1);
63 quat = new Matrix(Quaternion.N_PARAMS, 1);
64 quatResult = new Matrix(Quaternion.N_PARAMS, 1);
65 k1 = new Matrix(Quaternion.N_PARAMS, 1);
66 omegaSkew = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
67 } catch (final AlgebraException ignore) {
68 // never happens
69 }
70 }
71
72 /**
73 * Gets type of this integrator.
74 *
75 * @return indicates type of this integrator.
76 */
77 @Override
78 public QuaternionStepIntegratorType getType() {
79 return QuaternionStepIntegratorType.EULER_METHOD;
80 }
81
82 /**
83 * Performs an integration step using Euler's method.
84 * More information available here:
85 * <a href="https://en.wikipedia.org/wiki/Euler_method">https://en.wikipedia.org/wiki/Euler_method</a>
86 *
87 * @param initialAttitude initial attitude.
88 * @param initialWx initial x-coordinate rotation velocity at initial timestamp expressed
89 * in radians per second (rad/s).
90 * @param initialWy initial y-coordinate rotation velocity at initial timestamp expressed
91 * in radians per second (rad/s).
92 * @param initialWz initial z-coordinate rotation velocity at initial timestamp expressed
93 * in radians per second (rad/s).*
94 * @param currentWx end x-coordinate rotation velocity at current timestamp expressed in
95 * radians per second (rad/s).
96 * @param currentWy end y-coordinate rotation velocity at current timestamp expressed in
97 * radians per second (rad/s).
98 * @param currentWz end z-coordinate rotation velocity at current timestamp expressed in
99 * radians per second (rad/s).
100 * @param dt time step expressed in seconds.
101 * @param result instance where result of integration will be stored.
102 * @throws RotationException if a numerical error occurs.
103 */
104 @Override
105 public void integrate(
106 final Quaternion initialAttitude,
107 final double initialWx, final double initialWy, final double initialWz,
108 final double currentWx, final double currentWy, final double currentWz,
109 final double dt, final Quaternion result) throws RotationException {
110 integrationStep(initialAttitude, initialWx, initialWy, initialWz, dt, result, omega0, quat, quatResult, k1,
111 omegaSkew);
112 }
113
114 /**
115 * Performs an integration step using Euler's method.
116 * More information available here:
117 * <a href="https://en.wikipedia.org/wiki/Euler_method">https://en.wikipedia.org/wiki/Euler_method</a>
118 * This method should only be used sporadically. For better performance, if this method needs
119 * to be called very frequently, a new instance of {@link EulerQuaternionStepIntegrator} should
120 * be used and the non-static method should be used instead.
121 *
122 * @param initialAttitude initial attitude.
123 * @param initialWx end x-coordinate rotation velocity at current timestamp expressed in
124 * radians per second (rad/s).
125 * @param initialWy end y-coordinate rotation velocity at current timestamp expressed in
126 * radians per second (rad/s).
127 * @param initialWz end z-coordinate rotation velocity at current timestamp expressed in
128 * radians per second (rad/s).
129 * @param dt time step expressed in seconds.
130 * @param result instance where result of integration will be stored.
131 * @throws RotationException if a numerical error occurs.
132 */
133 public static void integrationStep(
134 final Quaternion initialAttitude,
135 final double initialWx, final double initialWy, final double initialWz,
136 final double dt, final Quaternion result) throws RotationException {
137
138 try {
139 final var omega0 = new Matrix(Rotation3D.INHOM_COORDS, 1);
140 final var quat = new Matrix(Quaternion.N_PARAMS, 1);
141 final var quatResult = new Matrix(Quaternion.N_PARAMS, 1);
142 final var k1 = new Matrix(Quaternion.N_PARAMS, 1);
143 final var omegaSkew = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
144 integrationStep(initialAttitude, initialWx, initialWy, initialWz, dt, result, omega0, quat, quatResult, k1,
145 omegaSkew);
146 } catch (final AlgebraException ignore) {
147 // never happens
148 }
149 }
150
151 /**
152 * Internal method computing an integration step using Euler's method.
153 * This method is used internally so that reusable instances can be provided as parameters.
154 *
155 * @param initialAttitude initial attitude.
156 * @param initialWx initial x-coordinate rotation velocity at current timestamp expressed
157 * in radians per second (rad/s).
158 * @param initialWy initial y-coordinate rotation velocity at current timestamp expressed
159 * in radians per second (rad/s).
160 * @param initialWz initial z-coordinate rotation velocity at current timestamp expressed
161 * in radians per second (rad/s).
162 * @param dt time step expressed in seconds.
163 * @param result instance where result of integration will be stored.
164 * @param omega0 angular speed at initial timestamp t0 to be reused. Must be 3x1.
165 * @param quat initial attitude to be reused. Must be 4x1.
166 * @param quatResult instance where result of integration is stored in matrix form being
167 * reused. Must be 4x1.
168 * @param k1 slope of quaternion derivative at initial timestamp t0 to be reused.
169 * Must be 4x1.
170 * @param omegaSkew skew antisymmetric matrix used for quaternion time derivative
171 * computation to be reused. Must be 4x4.
172 * @throws RotationException if a numerical error occurs.
173 */
174 private static void integrationStep(
175 final Quaternion initialAttitude, final double initialWx, final double initialWy, final double initialWz,
176 final double dt, final Quaternion result, final Matrix omega0, final Matrix quat,
177 final Matrix quatResult, final Matrix k1, final Matrix omegaSkew) throws RotationException {
178 try {
179 // normalize and copy initial attitude into matrix form
180 initialAttitude.normalize();
181 initialAttitude.values(quat.getBuffer());
182
183 // angular speed at initial timestamp t0
184 copyAngularSpeedToMatrix(initialWx, initialWy, initialWz, omega0);
185
186 // Compute slope k1 at initial point: k1 = f(t(n), x(n))
187 computeOmegaSkew(omega0, omegaSkew);
188 computeTimeDerivative(quat, omegaSkew, k1);
189
190 // Euler method follows expression:
191 // x(n + 1) = x(n) + dt * k1
192 k1.multiplyByScalar(dt);
193 quatResult.copyFrom(k1);
194 quatResult.add(quat);
195
196 result.setValues(quatResult.getBuffer());
197 result.normalize();
198 } catch (final AlgebraException e) {
199 throw new RotationException(e);
200 }
201 }
202 }