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 one step of a Runge-Kutta (RK4) integration algorithm.
26 * More information available here:
27 * <a href="https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods">
28 * https://en.wikipedia.org/wiki/Runge–Kutta_methods
29 * </a>
30 */
31 public class RungeKuttaQuaternionStepIntegrator extends QuaternionStepIntegrator {
32
33 /**
34 * Factor to multiply slopes k1 and k4.
35 */
36 private static final double ONE_SIXTH = 1.0 / 6.0;
37
38 /**
39 * Factor to multiply slopes k2 and k3.
40 */
41 private static final double ONE_THIRD = 1.0 / 3.0;
42
43 /**
44 * Angular speed at initial timestamp t0 to be reused.
45 */
46 private Matrix omega0;
47
48 /**
49 * Angular speed at end timestamp t1 to be reused.
50 */
51 private Matrix omega1;
52
53 /**
54 * Angular speed at mid-point timestamp between t0 and t1 to be reused.
55 */
56 private Matrix omega01;
57
58 /**
59 * Initial attitude to be reused.
60 */
61 private Matrix quat;
62
63 /**
64 * Instance where result of integration is stored in matrix form being reused.
65 */
66 private Matrix quatResult;
67
68 /**
69 * Temporal quaternion used to compute additional slopes that is being reused.
70 */
71 private Matrix tmpQ;
72
73 /**
74 * First Runge-Kutta coefficient. Quaternion derivative at initial timestamp t0 to be
75 * reused.
76 */
77 private Matrix k1;
78
79 /**
80 * Second Runge-Kutta coefficient. Quaternion derivative at mid-point timestamp
81 * between t0 and t1 to be reused.
82 */
83 private Matrix k2;
84
85 /**
86 * Third Runge-Kutta coefficient to be reused.
87 */
88 private Matrix k3;
89
90 /**
91 * Fourth Runge-Kutta coefficient. Quaternion derivative at end timestamp t1 to be
92 * reused.
93 */
94 private Matrix k4;
95
96 /**
97 * Skew antisymmetric matrix used for quaternion time derivative computation to be reused.
98 */
99 private Matrix omegaSkew;
100
101 /**
102 * Constructor.
103 * Initializes matrices being reused.
104 */
105 public RungeKuttaQuaternionStepIntegrator() {
106 try {
107 omega0 = new Matrix(Rotation3D.INHOM_COORDS, 1);
108 omega1 = new Matrix(Rotation3D.INHOM_COORDS, 1);
109 omega01 = new Matrix(Rotation3D.INHOM_COORDS, 1);
110 quat = new Matrix(Quaternion.N_PARAMS, 1);
111 quatResult = new Matrix(Quaternion.N_PARAMS, 1);
112 tmpQ = new Matrix(Quaternion.N_PARAMS, 1);
113 k1 = new Matrix(Quaternion.N_PARAMS, 1);
114 k2 = new Matrix(Quaternion.N_PARAMS, 1);
115 k3 = new Matrix(Quaternion.N_PARAMS, 1);
116 k4 = new Matrix(Quaternion.N_PARAMS, 1);
117 omegaSkew = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
118 } catch (final AlgebraException ignore) {
119 // never happens
120 }
121 }
122
123 /**
124 * Gets type of this integrator.
125 *
126 * @return indicates type of this integrator.
127 */
128 @Override
129 public QuaternionStepIntegratorType getType() {
130 return QuaternionStepIntegratorType.RUNGE_KUTTA;
131 }
132
133 /**
134 * Performs an RK4 Runge-Kutta integration step.
135 * More information available here:
136 * <a href="https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods">
137 * https://en.wikipedia.org/wiki/Runge–Kutta_methods
138 * </a>–
139 *
140 * @param initialAttitude initial attitude.
141 * @param initialWx initial x-coordinate rotation velocity at initial timestamp expressed in
142 * radians per second (rad/s).
143 * @param initialWy initial y-coordinate rotation velocity at initial timestamp expressed in
144 * radians per second (rad/s).
145 * @param initialWz initial z-coordinate rotation velocity at initial timestamp expressed in
146 * radians per second (rad/s).*
147 * @param currentWx end x-coordinate rotation velocity at current timestamp expressed in radians
148 * per second (rad/s).
149 * @param currentWy end y-coordinate rotation velocity at current timestamp expressed in radians
150 * per second (rad/s).
151 * @param currentWz end z-coordinate rotation velocity at current timestamp expressed in radians
152 * per second (rad/s).
153 * @param dt time step expressed in seconds.
154 * @param result instance where result of integration will be stored.
155 * @throws RotationException if a numerical error occurs.
156 */
157 @Override
158 public void integrate(
159 final Quaternion initialAttitude, final double initialWx, final double initialWy, final double initialWz,
160 final double currentWx, final double currentWy, final double currentWz, final double dt,
161 final Quaternion result) throws RotationException {
162 integrationStep(initialAttitude, initialWx, initialWy, initialWz, currentWx, currentWy, currentWz, dt, result,
163 omega0, omega1, omega01, quat, quatResult, tmpQ, k1, k2, k3, k4, omegaSkew);
164 }
165
166 /**
167 * Performs an RK4 Runge-Kutta integration step.
168 * More information available here:
169 * <a href="https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods">
170 * https://en.wikipedia.org/wiki/Runge–Kutta_methods
171 * </a>
172 *
173 * @param initialAttitude initial attitude.
174 * @param initialWx initial x-coordinate rotation velocity at initial timestamp expressed in
175 * radians per second (rad/s).
176 * @param initialWy initial y-coordinate rotation velocity at initial timestamp expressed in
177 * radians per second (rad/s).
178 * @param initialWz initial z-coordinate rotation velocity at initial timestamp expressed in
179 * radians per second (rad/s).
180 * @param currentWx end x-coordinate rotation velocity at end timestamp expressed in radians
181 * per second (rad/s).
182 * @param currentWy end y-coordinate rotation velocity at end timestamp expressed in radians
183 * per second (rad/s).
184 * @param currentWz end z-coordinate rotation velocity at end timestamp expressed in radians
185 * per second (rad/s).
186 * @param dt time step expressed in seconds (t1 - t0).
187 * @param result instance where result of integration will be stored.
188 * @throws RotationException if a numerical error occurs.
189 */
190 public static void integrationStep(
191 final Quaternion initialAttitude, final double initialWx, final double initialWy, final double initialWz,
192 final double currentWx, final double currentWy, final double currentWz, final double dt,
193 final Quaternion result) throws RotationException {
194 try {
195 final var omega0 = new Matrix(Rotation3D.INHOM_COORDS, 1);
196 final var omega1 = new Matrix(Rotation3D.INHOM_COORDS, 1);
197 final var omega01 = new Matrix(Rotation3D.INHOM_COORDS, 1);
198 final var quat = new Matrix(Quaternion.N_PARAMS, 1);
199 final var quatResult = new Matrix(Quaternion.N_PARAMS, 1);
200 final var tmpQ = new Matrix(Quaternion.N_PARAMS, 1);
201 final var k1 = new Matrix(Quaternion.N_PARAMS, 1);
202 final var k2 = new Matrix(Quaternion.N_PARAMS, 1);
203 final var k3 = new Matrix(Quaternion.N_PARAMS, 1);
204 final var k4 = new Matrix(Quaternion.N_PARAMS, 1);
205 final var omegaSkew = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
206 integrationStep(initialAttitude, initialWx, initialWy, initialWz, currentWx, currentWy, currentWz, dt,
207 result, omega0, omega1, omega01, quat, quatResult, tmpQ, k1, k2, k3, k4, omegaSkew);
208 } catch (final AlgebraException ignore) {
209 // never happens
210 }
211 }
212
213 /**
214 * Performs an RK4 Runge-Kutta integration step.
215 * More information available here:
216 * <a href="https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods">
217 * https://en.wikipedia.org/wiki/Runge–Kutta_methods
218 * </a>
219 *
220 * @param initialAttitude initial attitude.
221 * @param initialWx initial x-coordinate rotation velocity at initial timestamp expressed in
222 * radians per second (rad/s).
223 * @param initialWy initial y-coordinate rotation velocity at initial timestamp expressed in
224 * radians per second (rad/s).
225 * @param initialWz initial z-coordinate rotation velocity at initial timestamp expressed in
226 * radians per second (rad/s).
227 * @param currentWx end x-coordinate rotation velocity at end timestamp expressed in radians
228 * per second (rad/s).
229 * @param currentWy end y-coordinate rotation velocity at end timestamp expressed in radians
230 * per second (rad/s).
231 * @param currentWz end z-coordinate rotation velocity at end timestamp expressed in radians
232 * per second (rad/s).
233 * @param dt time step expressed in seconds (t1 - t0).
234 * @param result instance where result of integration will be stored.
235 * @param omega0 angular speed at initial timestamp t0 to be reused. Must be 3x1.
236 * @param omega1 angular speed at end timestamp t1 to be reused. Must be 3x1.
237 * @param omega01 angular speed at mid-point timestamp between t0 and t1 to be reused.
238 * Must be 3x1.
239 * @param quat initial attitude to be reused. Must be 4x1.
240 * @param quatResult instance where result of integration is stored in matrix form being
241 * reused. Must be 4x1.
242 * @param tmpQ temporal quaternion used to compute additional slopes that is being
243 * reused. Must be 4x1.
244 * @param k1 slope of quaternion derivative at initial timestamp t0 to be reused.
245 * Must be 4x1.
246 * @param k2 slope of quaternion derivative at mid-point timestamp between t0 and
247 * t1 to be reused. Must be 4x1.
248 * @param k3 slope of quaternion derivative at mid-point timestamp between t0 and
249 * t1 to be reused. Must be 4x1.
250 * @param k4 slope of quaternion derivative at end timestamp t1 to be reused.
251 * Must be 4x1.
252 * @param omegaSkew skew antisymmetric matrix used for quaternion time derivative
253 * computation to be reused. Must be 4x4.
254 * @throws RotationException if a numerical error occurs.
255 */
256 private static void integrationStep(
257 final Quaternion initialAttitude, final double initialWx, final double initialWy, final double initialWz,
258 final double currentWx, final double currentWy, final double currentWz, final double dt, Quaternion result,
259 final Matrix omega0, final Matrix omega1, final Matrix omega01, final Matrix quat, final Matrix quatResult,
260 final Matrix tmpQ, final Matrix k1, final Matrix k2, final Matrix k3, final Matrix k4,
261 final Matrix omegaSkew) throws RotationException {
262 try {
263 // normalize and copy initial attitude into matrix form
264 initialAttitude.normalize();
265 initialAttitude.values(quat.getBuffer());
266
267 // angular speed at initial timestamp t0
268 copyAngularSpeedToMatrix(initialWx, initialWy, initialWz, omega0);
269
270 // angular speed at end timestamp t1
271 copyAngularSpeedToMatrix(currentWx, currentWy, currentWz, omega1);
272
273 // compute average of angular speeds at mid-point between t0 and t1
274 computeAverageAngularSpeed(initialWx, initialWy, initialWz, currentWx, currentWy, currentWz, omega01);
275
276 // Compute First Runge-Kutta coefficient k1 as the slope at initial point: k1 = f(t(n), x(n))
277 // so that x(t(n) + 0.5 * dt) = x(n) + 0.5 * dt * k1
278 computeOmegaSkew(omega0, omegaSkew);
279 computeTimeDerivative(quat, omegaSkew, k1);
280
281 // Compute Second Runge-Kutta coefficient k2 as the slope at mid-point:
282 // k2 = f(t(n) + 0.5 * dt, x(t(n) + 0.5 * dt))
283 // k2 = f(t(n) + 0.5 * dt, x(n) + 0.5 * dt * k1)
284 final var halfDt = 0.5 * dt;
285 tmpQ.copyFrom(k1);
286 tmpQ.multiplyByScalar(halfDt);
287 tmpQ.add(quat);
288 computeOmegaSkew(omega01, omegaSkew);
289 computeTimeDerivative(tmpQ, omegaSkew, k2);
290
291 // Third Runge-Kutta coefficient k3 as the slope at mid-point (same omega skew as k2):
292 // k3 = f(t(n) + 0.5 * dt, x(t(n) + 0.5 * dt))
293 // k3 = f(t(n) + 0.5 * dt, x(n) + 0.5 * dt * k2)
294 tmpQ.copyFrom(k2);
295 tmpQ.multiplyByScalar(halfDt);
296 tmpQ.add(quat);
297 computeTimeDerivative(tmpQ, omegaSkew, k3);
298
299 // Fourth Runge-Kutta coefficient k4 as the slope at end-point:
300 // k4 = f(t(n) + dt, x(t(n) + dt))
301 // k4 = f(t(n) + dt, x(n) + dt * k3)
302 tmpQ.copyFrom(k3);
303 tmpQ.multiplyByScalar(dt);
304 tmpQ.add(quat);
305 computeOmegaSkew(omega1, omegaSkew);
306 computeTimeDerivative(tmpQ, omegaSkew, k4);
307
308 // result = quat + dt * (ONE_SIXTH * k1 + ONE_THIRD * k2 + ONE_THIRD * k3 + ONE_SIXTH * k4)
309 k1.multiplyByScalar(ONE_SIXTH);
310 k2.multiplyByScalar(ONE_THIRD);
311 k3.multiplyByScalar(ONE_THIRD);
312 k4.multiplyByScalar(ONE_SIXTH);
313
314 quatResult.copyFrom(k1);
315 quatResult.add(k2);
316 quatResult.add(k3);
317 quatResult.add(k4);
318 quatResult.multiplyByScalar(dt);
319 quatResult.add(quat);
320
321 result.setValues(quatResult.getBuffer());
322 result.normalize();
323 } catch (final AlgebraException e) {
324 throw new RotationException(e);
325 }
326 }
327 }