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 mid-point algorithm.
26 * More information available here:
27 * <a href="https://en.wikipedia.org/wiki/Midpoint_method">https://en.wikipedia.org/wiki/Midpoint_method</a>
28 */
29 public class MidPointQuaternionStepIntegrator extends QuaternionStepIntegrator {
30
31 /**
32 * Angular speed at initial timestamp t0 to be reused.
33 */
34 private Matrix omega0;
35
36 /**
37 * Angular speed at end timestamp t1 to be reused.
38 */
39 private Matrix omega1;
40
41 /**
42 * Angular speed at mid-point timestamp between t0 and t1 to be reused.
43 */
44 private Matrix omega01;
45
46 /**
47 * Initial attitude to be reused.
48 */
49 private Matrix quat;
50
51 /**
52 * Instance where result of integration is stored in matrix form being reused.
53 */
54 private Matrix quatResult;
55
56 /**
57 * Temporal quaternion used to compute additional slopes that is being reused.
58 */
59 private Matrix tmpQ;
60
61 /**
62 * Quaternion derivative at initial timestamp t0 to be reused.
63 */
64 private Matrix k1;
65
66 /**
67 * Quaternion derivative at mid-point timestamp between t0 and t1 to be reused.
68 */
69 private Matrix k2;
70
71 /**
72 * Skew antisymmetric matrix used for quaternion time derivative computation to be reused.
73 */
74 private Matrix omegaSkew;
75
76 /**
77 * Constructor.
78 * Initializes matrices being reused.
79 */
80 public MidPointQuaternionStepIntegrator() {
81 try {
82 omega0 = new Matrix(Rotation3D.INHOM_COORDS, 1);
83 omega1 = new Matrix(Rotation3D.INHOM_COORDS, 1);
84 omega01 = new Matrix(Rotation3D.INHOM_COORDS, 1);
85 quat = new Matrix(Quaternion.N_PARAMS, 1);
86 quatResult = new Matrix(Quaternion.N_PARAMS, 1);
87 tmpQ = new Matrix(Quaternion.N_PARAMS, 1);
88 k1 = new Matrix(Quaternion.N_PARAMS, 1);
89 k2 = new Matrix(Quaternion.N_PARAMS, 1);
90 omegaSkew = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
91 } catch (final AlgebraException ignore) {
92 // never happens
93 }
94 }
95
96 /**
97 * Gets type of this integrator.
98 *
99 * @return indicates type of this integrator.
100 */
101 @Override
102 public QuaternionStepIntegratorType getType() {
103 return QuaternionStepIntegratorType.MID_POINT;
104 }
105
106 /**
107 * Performs a mid-point integration step.
108 * More information available here:
109 * <a href="https://en.wikipedia.org/wiki/Midpoint_method">https://en.wikipedia.org/wiki/Midpoint_method</a>
110 *
111 * @param initialAttitude initial attitude.
112 * @param initialWx initial x-coordinate rotation velocity at initial timestamp expressed
113 * in radians per second (rad/s).
114 * @param initialWy initial y-coordinate rotation velocity at initial timestamp expressed
115 * in radians per second (rad/s).
116 * @param initialWz initial z-coordinate rotation velocity at initial timestamp expressed
117 * in radians per second (rad/s).*
118 * @param currentWx end x-coordinate rotation velocity at current timestamp expressed in
119 * radians per second (rad/s).
120 * @param currentWy end y-coordinate rotation velocity at current timestamp expressed in
121 * radians per second (rad/s).
122 * @param currentWz end z-coordinate rotation velocity at current timestamp expressed in
123 * radians per second (rad/s).
124 * @param dt time step expressed in seconds.
125 * @param result instance where result of integration will be stored.
126 * @throws RotationException if a numerical error occurs.
127 */
128 @Override
129 public void integrate(final Quaternion initialAttitude,
130 final double initialWx, final double initialWy, final double initialWz,
131 final double currentWx, final double currentWy, final double currentWz,
132 final double dt, final Quaternion result) throws RotationException {
133 integrationStep(initialAttitude, initialWx, initialWy, initialWz, currentWx, currentWy, currentWz, dt, result,
134 omega0, omega1, omega01, quat, quatResult, tmpQ, k1, k2, omegaSkew);
135 }
136
137 /**
138 * Performs a mid-point integration step.
139 * More information available here:
140 * <a href="https://en.wikipedia.org/wiki/Midpoint_method">https://en.wikipedia.org/wiki/Midpoint_method</a>
141 *
142 * @param initialAttitude initial attitude.
143 * @param initialWx initial x-coordinate rotation velocity at initial timestamp expressed
144 * in radians per second (rad/s).
145 * @param initialWy initial y-coordinate rotation velocity at initial timestamp expressed
146 * in radians per second (rad/s).
147 * @param initialWz initial z-coordinate rotation velocity at initial timestamp expressed
148 * in radians per second (rad/s).
149 * @param currentWx end x-coordinate rotation velocity at end timestamp expressed in
150 * radians per second (rad/s).
151 * @param currentWy end y-coordinate rotation velocity at end timestamp expressed in
152 * radians per second (rad/s).
153 * @param currentWz end z-coordinate rotation velocity at end timestamp expressed in
154 * radians per second (rad/s).
155 * @param dt time step expressed in seconds (t1 - t0).
156 * @param result instance where result of integration will be stored.
157 * @throws RotationException if a numerical error occurs.
158 */
159 public static void integrationStep(
160 final Quaternion initialAttitude,
161 final double initialWx, final double initialWy, final double initialWz,
162 final double currentWx, final double currentWy, final double currentWz,
163 final double dt, final Quaternion result) throws RotationException {
164 try {
165 final var omega0 = new Matrix(Rotation3D.INHOM_COORDS, 1);
166 final var omega1 = new Matrix(Rotation3D.INHOM_COORDS, 1);
167 final var omega01 = new Matrix(Rotation3D.INHOM_COORDS, 1);
168 final var quat = new Matrix(Quaternion.N_PARAMS, 1);
169 final var quatResult = new Matrix(Quaternion.N_PARAMS, 1);
170 final var tmpQ = new Matrix(Quaternion.N_PARAMS, 1);
171 final var k1 = new Matrix(Quaternion.N_PARAMS, 1);
172 final var k2 = new Matrix(Quaternion.N_PARAMS, 1);
173 final var omegaSkew = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
174 integrationStep(initialAttitude, initialWx, initialWy, initialWz, currentWx, currentWy, currentWz, dt,
175 result, omega0, omega1, omega01, quat, quatResult, tmpQ, k1, k2, omegaSkew);
176 } catch (final AlgebraException ignore) {
177 // never happens
178 }
179 }
180
181 /**
182 * Internal method computing an integration step using mid-point algorithm.
183 * This method is used internally so that reusable instances can be provided as parameters.
184 *
185 * @param initialAttitude initial attitude.
186 * @param initialWx initial x-coordinate rotation velocity at initial timestamp expressed
187 * in radians per second (rad/s).
188 * @param initialWy initial y-coordinate rotation velocity at initial timestamp expressed
189 * in radians per second (rad/s).
190 * @param initialWz initial z-coordinate rotation velocity at initial timestamp expressed
191 * in radians per second (rad/s).
192 * @param currentWx end x-coordinate rotation velocity at end timestamp expressed in
193 * radians per second (rad/s).
194 * @param currentWy end y-coordinate rotation velocity at end timestamp expressed in
195 * radians per second (rad/s).
196 * @param currentWz end z-coordinate rotation velocity at end timestamp expressed in
197 * radians per second (rad/s).
198 * @param dt time step expressed in seconds (t1 - t0).
199 * @param result instance where result of integration will be stored.
200 * @param omega0 angular speed at initial timestamp t0 to be reused. Must be 3x1.
201 * @param omega1 angular speed at end timestamp t1 to be reused. Must be 3x1.
202 * @param omega01 angular speed at mid-point timestamp between t0 and t1 to be reused.
203 * Must be 3x1.
204 * @param quat initial attitude to be reused. Must be 4x1.
205 * @param quatResult instance where result of integration is stored in matrix form being
206 * reused. Must be 4x1.
207 * @param tmpQ temporal quaternion used to compute additional slopes that is being
208 * reused. Must be 4x1.
209 * @param k1 slope of quaternion derivative at initial timestamp t0 to be reused.
210 * Must be 4x1.
211 * @param k2 slope of quaternion derivative at mid-point timestamp between t0 and
212 * t1 to be reused. Must be 4x1.
213 * @param omegaSkew skew antisymmetric matrix used for quaternion time derivative
214 * computation to be reused. Must be 4x4.
215 * @throws RotationException if a numerical error occurs.
216 */
217 private static void integrationStep(
218 final Quaternion initialAttitude,
219 final double initialWx, final double initialWy, final double initialWz,
220 final double currentWx, final double currentWy, final double currentWz,
221 final double dt, final Quaternion result, final Matrix omega0, final Matrix omega1,
222 final Matrix omega01, final Matrix quat, final Matrix quatResult, final Matrix tmpQ,
223 final Matrix k1, final Matrix k2, final Matrix omegaSkew) throws RotationException {
224 try {
225 // normalize and copy initial attitude into matrix form
226 initialAttitude.normalize();
227 initialAttitude.values(quat.getBuffer());
228
229 // angular speed at initial timestamp t0
230 copyAngularSpeedToMatrix(initialWx, initialWy, initialWz, omega0);
231
232 // angular speed at end timestamp t1
233 copyAngularSpeedToMatrix(currentWx, currentWy, currentWz, omega1);
234
235 // compute average of angular speeds at mid-point between t0 and t1
236 computeAverageAngularSpeed(initialWx, initialWy, initialWz, currentWx, currentWy, currentWz, omega01);
237
238 // Compute slope k1 at initial point: k1 = f(t(n), x(n))
239 // so that x(t(n) + 0.5 * dt) = x(n) + 0.5 * dt * k1
240 computeOmegaSkew(omega0, omegaSkew);
241 computeTimeDerivative(quat, omegaSkew, k1);
242
243 // Compute slope k2 at mid-point:
244 // k2 = f(t(n) + 0.5 * dt, x(t(n) + 0.5 * dt))
245 // k2 = f(t(n) + 0.5 * dt, x(n) + 0.5 * dt * k1)
246 tmpQ.copyFrom(k1);
247 tmpQ.multiplyByScalar(0.5 * dt);
248 tmpQ.add(quat);
249 computeOmegaSkew(omega01, omegaSkew);
250 computeTimeDerivative(tmpQ, omegaSkew, k2);
251
252 // Mid-point method follows expression:
253 // x(n + 1) = x(n) + dt * k2
254 k2.multiplyByScalar(dt);
255 quatResult.copyFrom(k2);
256 quatResult.add(quat);
257
258 result.setValues(quatResult.getBuffer());
259 result.normalize();
260 } catch (final AlgebraException e) {
261 throw new RotationException(e);
262 }
263 }
264 }