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.Matrix;
19 import com.irurueta.algebra.WrongSizeException;
20 import com.irurueta.geometry.Quaternion;
21 import com.irurueta.geometry.RotationException;
22
23 /**
24 * Contains common methods and factory methods for all implementations of this class.
25 */
26 public abstract class QuaternionStepIntegrator {
27
28 /**
29 * Default quaternion step integrator type.
30 */
31 public static final QuaternionStepIntegratorType DEFAULT_TYPE = QuaternionStepIntegratorType.RUNGE_KUTTA;
32
33 /**
34 * Gets type of this integrator.
35 *
36 * @return indicates type of this integrator.
37 */
38 public abstract QuaternionStepIntegratorType getType();
39
40 /**
41 * Performs an integration step.
42 *
43 * @param initialAttitude initial attitude.
44 * @param initialWx initial x-coordinate rotation velocity at initial timestamp expressed in
45 * radians per second (rad/s).
46 * @param initialWy initial y-coordinate rotation velocity at initial timestamp expressed in
47 * radians per second (rad/s).
48 * @param initialWz initial z-coordinate rotation velocity at initial timestamp expressed in
49 * radians per second (rad/s).*
50 * @param currentWx end x-coordinate rotation velocity at current timestamp expressed in radians
51 * per second (rad/s).
52 * @param currentWy end y-coordinate rotation velocity at current timestamp expressed in radians
53 * per second (rad/s).
54 * @param currentWz end z-coordinate rotation velocity at current timestamp expressed in radians
55 * per second (rad/s).
56 * @param dt time step expressed in seconds.
57 * @param result instance where result of integration will be stored.
58 * @throws RotationException if a numerical error occurs.
59 */
60 public abstract void integrate(
61 final Quaternion initialAttitude, final double initialWx, final double initialWy, final double initialWz,
62 final double currentWx, final double currentWy, final double currentWz, final double dt,
63 final Quaternion result) throws RotationException;
64
65 /**
66 * Creates a quaternion step integrator using provided type.
67 *
68 * @param type type of quaternion step integrator.
69 * @return created quaternion step integrator.
70 */
71 public static QuaternionStepIntegrator create(QuaternionStepIntegratorType type) {
72 return switch (type) {
73 case EULER_METHOD -> new EulerQuaternionStepIntegrator();
74 case MID_POINT -> new MidPointQuaternionStepIntegrator();
75 case SUH -> new SuhQuaternionStepIntegrator();
76 case TRAWNY -> new TrawnyQuaternionStepIntegrator();
77 case YUAN -> new YuanQuaternionStepIntegrator();
78 default -> new RungeKuttaQuaternionStepIntegrator();
79 };
80 }
81
82 /**
83 * Creates a quaternion step integrator using default type.
84 *
85 * @return created quaternion step integrator.
86 */
87 public static QuaternionStepIntegrator create() {
88 return create(DEFAULT_TYPE);
89 }
90
91 /**
92 * Computes the time derivative of a quaternion at a given angular speed.
93 *
94 * @param quaternion column vector matrix containing quaternion to compute time derivative for.
95 * Must be 4x1.
96 * @param omegaSkew instance being reused containing the skew antisymmetric matrix. Must be 4x4.
97 * @param result instance where computed time derivative will be stored. Must be 4x1.
98 * @throws WrongSizeException if provided matrices do not have proper size.
99 */
100 protected static void computeTimeDerivative(
101 final Matrix quaternion, final Matrix omegaSkew, final Matrix result) throws WrongSizeException {
102
103 // The time derivative of a quaternion at a given angular speed follows expression:
104 // q`= 0.5 * W * q
105 // where W is the skew antisymmetric matrix of provided angular speed and q is a quaternion
106 // containing a given attitude.
107 omegaSkew.multiply(quaternion, result);
108 result.multiplyByScalar(0.5);
109 }
110
111 /**
112 * Computes the skew antisymmetric matrix of a vector matrix containing angular speed.
113 *
114 * @param omega column vector matrix containing angular speed. Must be 3x1.
115 * @param result instance where result must be stored. Must be 4x4.
116 */
117 protected static void computeOmegaSkew(final Matrix omega, final Matrix result) {
118
119 final var wx = omega.getElementAtIndex(0);
120 final var wy = omega.getElementAtIndex(1);
121 final var wz = omega.getElementAtIndex(2);
122
123 // The skew matrix has the following expression:
124 // W = [0 -wx -wy -wz]
125 // [wx 0 wz -wy]
126 // [wy -wz 0 wx]
127 // [wz wy -wx 0]
128
129 result.setElementAtIndex(0, 0.0);
130 result.setElementAtIndex(1, wx);
131 result.setElementAtIndex(2, wy);
132 result.setElementAtIndex(3, wz);
133
134 result.setElementAtIndex(4, -wx);
135 result.setElementAtIndex(5, 0.0);
136 result.setElementAtIndex(6, -wz);
137 result.setElementAtIndex(7, wy);
138
139 result.setElementAtIndex(8, -wy);
140 result.setElementAtIndex(9, wz);
141 result.setElementAtIndex(10, 0.0);
142 result.setElementAtIndex(11, -wx);
143
144 result.setElementAtIndex(12, -wz);
145 result.setElementAtIndex(13, -wy);
146 result.setElementAtIndex(14, wx);
147 result.setElementAtIndex(15, 0.0);
148 }
149
150 /**
151 * Copies provided angular speed coordinates into a matrix.
152 *
153 * @param wx x-coordinate of angular speed expressed in radians per second (rad/s).
154 * @param wy y-coordinate of angular speed expressed in radians per second (rad/s).
155 * @param wz z-coordinate of angular speed expressed in radians per second (rad/s).
156 * @param result instance where angular speed coordinates are copied to.
157 */
158 protected static void copyAngularSpeedToMatrix(
159 final double wx, final double wy, final double wz, final Matrix result) {
160 result.setElementAtIndex(0, wx);
161 result.setElementAtIndex(1, wy);
162 result.setElementAtIndex(2, wz);
163 }
164
165 /**
166 * Computes average angular speed into matrix form at mid-point between initial timestamp t0
167 * and end timestamp t1.
168 *
169 * @param initialWx initial x-coordinate rotation velocity at initial timestamp expressed
170 * in radians per second (rad/s).
171 * @param initialWy initial y-coordinate rotation velocity at initial timestamp expressed
172 * in radians per second (rad/s).
173 * @param initialWz initial z-coordinate rotation velocity at initial timestamp expressed
174 * in radians per second (rad/s).*
175 * @param currentWx end x-coordinate rotation velocity at current timestamp expressed in
176 * radians per second (rad/s).
177 * @param currentWy end y-coordinate rotation velocity at current timestamp expressed in
178 * radians per second (rad/s).
179 * @param currentWz end z-coordinate rotation velocity at current timestamp expressed in
180 * radians per second (rad/s).
181 * @param result instance where result will be stored. Must be 3x1.
182 */
183 protected static void computeAverageAngularSpeed(
184 final double initialWx, final double initialWy, final double initialWz,
185 final double currentWx, final double currentWy, final double currentWz, final Matrix result) {
186 result.setElementAtIndex(0, 0.5 * (initialWx + currentWx));
187 result.setElementAtIndex(1, 0.5 * (initialWy + currentWy));
188 result.setElementAtIndex(2, 0.5 * (initialWz + currentWz));
189 }
190 }