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.Matrix;
20 import com.irurueta.geometry.Quaternion;
21 import com.irurueta.geometry.RotationException;
22
23 /**
24 * Computes an integration step of a quaternion using Suh's method.
25 * More information available here:
26 * Yuan, S. "Quaternion-based Unscented Kalman Filter for Real-time". 2015,
27 */
28 public class YuanQuaternionStepIntegrator extends QuaternionStepIntegrator {
29
30 private static final double EPSILON = 1e-15;
31
32 /**
33 * Angular speed at initial timestamp t0 to be reused.
34 */
35 private Matrix quat;
36
37 /**
38 * Angular speed at initial timestamp t0 to be reused.
39 */
40 private Matrix omega;
41
42 /**
43 * Temporary matrix to be reused.
44 */
45 private Matrix a;
46
47 /**
48 * Identity matrix to be reused.
49 */
50 private Matrix identity;
51
52 /**
53 * Temporary matrix to be reused.
54 */
55 private Matrix tmp;
56
57 /**
58 * Instance where result of integration is stored in matrix form being reused.
59 */
60 private Matrix quatResult;
61
62 /**
63 * Constructor.
64 * Initializes matrices being reused.
65 */
66 public YuanQuaternionStepIntegrator() {
67 try {
68 quat = new Matrix(Quaternion.N_PARAMS, 1);
69 omega = new Matrix(Quaternion.N_ANGLES, 1);
70 a = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
71 identity = Matrix.identity(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
72 tmp = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
73 quatResult = new Matrix(Quaternion.N_PARAMS, 1);
74 } catch (final AlgebraException ignore) {
75 // never happens
76 }
77 }
78
79 /**
80 * Gets type of this integrator.
81 *
82 * @return indicates type of this integrator.
83 */
84 @Override
85 public QuaternionStepIntegratorType getType() {
86 return QuaternionStepIntegratorType.YUAN;
87 }
88
89 /**
90 * Performs Yuan's integration step.
91 * More information available here:
92 * Yuan, S. "Quaternion-based Unscented Kalman Filter for Real-time". 2015,
93 *
94 * @param initialAttitude initial attitude.
95 * @param initialWx initial x-coordinate rotation velocity at initial timestamp expressed
96 * in radians per second (rad/s).
97 * @param initialWy initial y-coordinate rotation velocity at initial timestamp expressed
98 * in radians per second (rad/s).
99 * @param initialWz initial z-coordinate rotation velocity at initial timestamp expressed
100 * in radians per second (rad/s).*
101 * @param currentWx end x-coordinate rotation velocity at current timestamp expressed in
102 * radians per second (rad/s).
103 * @param currentWy end y-coordinate rotation velocity at current timestamp expressed in
104 * radians per second (rad/s).
105 * @param currentWz end z-coordinate rotation velocity at current timestamp expressed in
106 * radians per second (rad/s).
107 * @param dt time step expressed in seconds.
108 * @param result instance where result of integration will be stored.
109 * @throws RotationException if a numerical error occurs.
110 */
111 @Override
112 public void integrate(
113 final Quaternion initialAttitude,
114 final double initialWx, final double initialWy, final double initialWz,
115 final double currentWx, final double currentWy, final double currentWz,
116 final double dt, final Quaternion result) throws RotationException {
117 integrationStep(initialAttitude, initialWx, initialWy, initialWz,
118 currentWx, currentWy, currentWz, dt, result, quat, omega, a, identity, tmp,
119 quatResult);
120 }
121
122 /**
123 * Performs Yuan's integration step.
124 * More information available here:
125 * Yuan, S. "Quaternion-based Unscented Kalman Filter for Real-time". 2015,
126 *
127 * @param initialAttitude initial attitude.
128 * @param initialWx initial x-coordinate rotation velocity at initial timestamp expressed
129 * in radians per second (rad/s).
130 * @param initialWy initial y-coordinate rotation velocity at initial timestamp expressed
131 * in radians per second (rad/s).
132 * @param initialWz initial z-coordinate rotation velocity at initial timestamp expressed
133 * in radians per second (rad/s).
134 * @param currentWx end x-coordinate rotation velocity at end timestamp expressed in
135 * radians per second (rad/s).
136 * @param currentWy end y-coordinate rotation velocity at end timestamp expressed in
137 * radians per second (rad/s).
138 * @param currentWz end z-coordinate rotation velocity at end timestamp expressed in
139 * radians per second (rad/s).
140 * @param dt time step expressed in seconds (t1 - t0).
141 * @param result instance where result of integration will be stored.
142 * @throws RotationException if a numerical error occurs.
143 */
144 public static void integrationStep(
145 final Quaternion initialAttitude,
146 final double initialWx, final double initialWy, final double initialWz,
147 final double currentWx, final double currentWy, final double currentWz,
148 final double dt, final Quaternion result) throws RotationException {
149 try {
150 final var quat = new Matrix(Quaternion.N_PARAMS, 1);
151 final var omega = new Matrix(Quaternion.N_ANGLES, 1);
152 final var a = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
153 final var identity = Matrix.identity(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
154 final var tmp = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
155 final var quatResult = new Matrix(Quaternion.N_PARAMS, 1);
156 integrationStep(initialAttitude, initialWx, initialWy, initialWz,
157 currentWx, currentWy, currentWz, dt, result, quat, omega, a, identity, tmp, quatResult);
158 } catch (final AlgebraException ignore) {
159 // never happens
160 }
161 }
162
163 /**
164 * Internal method computing an integration step using Trawny's algorithm.
165 * This method is used internally so that reusable instances can be provided as parameters.
166 *
167 * @param initialAttitude initial attitude.
168 * @param initialWx initial x-coordinate rotation velocity at initial timestamp expressed
169 * in radians per second (rad/s).
170 * @param initialWy initial y-coordinate rotation velocity at initial timestamp expressed
171 * in radians per second (rad/s).
172 * @param initialWz initial z-coordinate rotation velocity at initial timestamp expressed
173 * in radians per second (rad/s).
174 * @param currentWx end x-coordinate rotation velocity at end timestamp expressed in
175 * radians per second (rad/s).
176 * @param currentWy end y-coordinate rotation velocity at end timestamp expressed in
177 * radians per second (rad/s).
178 * @param currentWz end z-coordinate rotation velocity at end timestamp expressed in
179 * radians per second (rad/s).
180 * @param dt time step expressed in seconds (t1 - t0).
181 * @param result instance where result of integration will be stored.
182 * @param quat initial attitude to be reused. Must be 4x1.
183 * @param omega angular speed at initial timestamp t0 to be reused. Must be 3x1.
184 * @param a temporary matrix to be reused. Must be 4x4.
185 * @param identity identity matrix to be reused. Must be 4x4.
186 * @param tmp temporary matrix to be reused. Must be 4x4.
187 * @param quatResult instance where result of integration is stored in matrix form being
188 * reused. Must be 4x1.
189 * @throws RotationException if a numerical error occurs.
190 */
191 private static void integrationStep(
192 final Quaternion initialAttitude,
193 final double initialWx, final double initialWy, final double initialWz,
194 final double currentWx, final double currentWy, final double currentWz,
195 final double dt, final Quaternion result, final Matrix quat, final Matrix omega,
196 final Matrix a, final Matrix identity, final Matrix tmp, final Matrix quatResult)
197 throws RotationException {
198 try {
199 // normalize and copy initial attitude into matrix form
200 initialAttitude.normalize();
201 initialAttitude.values(quat.getBuffer());
202
203 final var w1 = (initialWx + currentWx) / 2.0;
204 final var w2 = (initialWy + currentWy) / 2.0;
205 final var w3 = (initialWz + currentWz) / 2.0;
206
207 copyAngularSpeedToMatrix(w1, w2, w3, omega);
208 computeOmegaSkew(omega, a);
209
210 final var w1dt = w1 * dt;
211 final var w2dt = w2 * dt;
212 final var w3dt = w3 * dt;
213
214 final var w1dt2 = w1dt * w1dt;
215 final var w2dt2 = w2dt * w2dt;
216 final var w3dt2 = w3dt * w3dt;
217
218 final var theta = Math.sqrt(w1dt2 + w2dt2 + w3dt2);
219 final var halfTheta = theta / 2;
220 final double sinc;
221 if (theta > EPSILON) {
222 sinc = Math.sin(halfTheta) / halfTheta;
223 } else {
224 // notice that sin(x) / x --> 1 for x --> 0
225 sinc = 1.0;
226 }
227
228 a.multiplyByScalar(0.5 * sinc * dt);
229
230 tmp.copyFrom(identity);
231 tmp.multiplyByScalar(Math.cos(halfTheta));
232 tmp.add(a);
233
234 tmp.multiply(quat, quatResult);
235
236 result.setValues(quatResult.getBuffer());
237 result.normalize();
238 } catch (final AlgebraException ex) {
239 throw new RotationException(ex);
240 }
241 }
242 }