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.Rotation3D;
22 import com.irurueta.geometry.RotationException;
23 import com.irurueta.numerical.ExponentialMatrixEstimator;
24
25 /**
26 * Computes an integration step of a quaternion using Suh's method.
27 * More information available here:
28 * Trawny, N. "Indirect Kalman Filter for 3D Attitude Estimation". 2005.
29 */
30 public class TrawnyQuaternionStepIntegrator extends QuaternionStepIntegrator {
31
32 /**
33 * Estimates exponential of a square matrix.
34 */
35 private final ExponentialMatrixEstimator exponentialMatrixEstimator = new ExponentialMatrixEstimator();
36
37 /**
38 * Initial attitude to be reused.
39 */
40 private Matrix quat;
41
42 /**
43 * Angular speed at initial timestamp t0 to be reused.
44 */
45 private Matrix omega0;
46
47 /**
48 * Angular speed at end timestamp t1 to be reused.
49 */
50 private Matrix omega1;
51
52 /**
53 * Skew matrix of omega0 to be reused.
54 */
55 private Matrix omegaSkew0;
56
57 /**
58 * Skew matrix of omega1 to be reused.
59 */
60 private Matrix omegaSkew1;
61
62 /**
63 * Average skew of angular speeds omega0 and omega1 to be reused.
64 */
65 private Matrix omegaAvgOmega;
66
67 /**
68 * Derivative of angular speed to be reused.
69 */
70 private Matrix dotOmega;
71
72 /**
73 * Skew matrix of derivative of angular speed to be reused.
74 */
75 private Matrix omegaSkewDotOmega;
76
77 /**
78 * Temporary matrix to be reused.
79 */
80 private Matrix a;
81
82 /**
83 * Temporary matrix to be reused.
84 */
85 private Matrix b;
86
87 /**
88 * Temporary matrix to be reused.
89 */
90 private Matrix tmp;
91
92 /**
93 * Exponential matrix to be reused.
94 */
95 private Matrix expA;
96
97 /**
98 * Instance where result of integration is stored in matrix form being reused.
99 */
100 private Matrix quatResult;
101
102 /**
103 * Constructor.
104 * Initializes matrices being reused.
105 */
106 public TrawnyQuaternionStepIntegrator() {
107 try {
108 quat = new Matrix(Quaternion.N_PARAMS, 1);
109 omega0 = new Matrix(Rotation3D.INHOM_COORDS, 1);
110 omega1 = new Matrix(Rotation3D.INHOM_COORDS, 1);
111 omegaSkew0 = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
112 omegaSkew1 = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
113 omegaAvgOmega = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
114 dotOmega = new Matrix(Rotation3D.INHOM_COORDS, 1);
115 omegaSkewDotOmega = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
116 a = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
117 b = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
118 tmp = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
119 expA = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
120 quatResult = new Matrix(Quaternion.N_PARAMS, 1);
121 } catch (final AlgebraException ignore) {
122 // never happens
123 }
124 }
125
126 /**
127 * Gets type of this integrator.
128 *
129 * @return indicates type of this integrator.
130 */
131 @Override
132 public QuaternionStepIntegratorType getType() {
133 return QuaternionStepIntegratorType.TRAWNY;
134 }
135
136 /**
137 * Performs Trawny's integration step.
138 * More information available here:
139 * Trawny, N. "Indirect Kalman Filter for 3D Attitude Estimation". 2005.
140 *
141 * @param initialAttitude initial attitude.
142 * @param initialWx initial x-coordinate rotation velocity at initial timestamp expressed
143 * in radians per second (rad/s).
144 * @param initialWy initial y-coordinate rotation velocity at initial timestamp expressed
145 * in radians per second (rad/s).
146 * @param initialWz initial z-coordinate rotation velocity at initial timestamp expressed
147 * in radians per second (rad/s).*
148 * @param currentWx end x-coordinate rotation velocity at current timestamp expressed in
149 * radians per second (rad/s).
150 * @param currentWy end y-coordinate rotation velocity at current timestamp expressed in
151 * radians per second (rad/s).
152 * @param currentWz end z-coordinate rotation velocity at current timestamp expressed in
153 * radians per second (rad/s).
154 * @param dt time step expressed in seconds.
155 * @param result instance where result of integration will be stored.
156 * @throws RotationException if a numerical error occurs.
157 */
158 @Override
159 public void integrate(
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 integrationStep(initialAttitude, initialWx, initialWy, initialWz,
165 currentWx, currentWy, currentWz, dt, result, exponentialMatrixEstimator, quat,
166 omega0, omega1, omegaSkew0, omegaSkew1, omegaAvgOmega, dotOmega,
167 omegaSkewDotOmega, a, b, tmp, expA, quatResult);
168 }
169
170 /**
171 * Performs Trawny's integration step.
172 * More information available here:
173 * Trawny, N. "Indirect Kalman Filter for 3D Attitude Estimation". 2005.
174 *
175 * @param initialAttitude initial attitude.
176 * @param initialWx initial x-coordinate rotation velocity at initial timestamp expressed
177 * in radians per second (rad/s).
178 * @param initialWy initial y-coordinate rotation velocity at initial timestamp expressed
179 * in radians per second (rad/s).
180 * @param initialWz initial z-coordinate rotation velocity at initial timestamp expressed
181 * in radians per second (rad/s).
182 * @param currentWx end x-coordinate rotation velocity at end timestamp expressed in
183 * radians per second (rad/s).
184 * @param currentWy end y-coordinate rotation velocity at end timestamp expressed in
185 * radians per second (rad/s).
186 * @param currentWz end z-coordinate rotation velocity at end timestamp expressed in
187 * radians per second (rad/s).
188 * @param dt time step expressed in seconds (t1 - t0).
189 * @param result instance where result of integration will be stored.
190 * @throws RotationException if a numerical error occurs.
191 */
192 public static void integrationStep(
193 final Quaternion initialAttitude,
194 final double initialWx, final double initialWy, final double initialWz,
195 final double currentWx, final double currentWy, final double currentWz,
196 final double dt, final Quaternion result) throws RotationException {
197 try {
198 final var exponentialMatrixEstimator = new ExponentialMatrixEstimator();
199 final var quat = new Matrix(Quaternion.N_PARAMS, 1);
200 final var omega0 = new Matrix(Rotation3D.INHOM_COORDS, 1);
201 final var omega1 = new Matrix(Rotation3D.INHOM_COORDS, 1);
202 final var omegaSkew0 = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
203 final var omegaSkew1 = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
204 final var omegaAvgOmega = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
205 final var dotOmega = new Matrix(Rotation3D.INHOM_COORDS, 1);
206 final var omegaSkewDotOmega = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
207 final var a = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
208 final var b = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
209 final var tmp = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
210 final var expA = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
211 final var quatResult = new Matrix(Quaternion.N_PARAMS, 1);
212 integrationStep(initialAttitude, initialWx, initialWy, initialWz,
213 currentWx, currentWy, currentWz, dt, result, exponentialMatrixEstimator, quat,
214 omega0, omega1, omegaSkew0, omegaSkew1, omegaAvgOmega, dotOmega,
215 omegaSkewDotOmega, a, b, tmp, expA, quatResult);
216 } catch (final AlgebraException ignore) {
217 // never happens
218 }
219 }
220
221 /**
222 * Internal method computing an integration step using Trawny's algorithm.
223 * This method is used internally so that reusable instances can be provided as parameters.
224 *
225 * @param initialAttitude initial attitude.
226 * @param initialWx initial x-coordinate rotation velocity at initial timestamp expressed
227 * in radians per second (rad/s).
228 * @param initialWy initial y-coordinate rotation velocity at initial timestamp expressed
229 * in radians per second (rad/s).
230 * @param initialWz initial z-coordinate rotation velocity at initial timestamp expressed
231 * in radians per second (rad/s).
232 * @param currentWx end x-coordinate rotation velocity at end timestamp expressed in
233 * radians per second (rad/s).
234 * @param currentWy end y-coordinate rotation velocity at end timestamp expressed in
235 * radians per second (rad/s).
236 * @param currentWz end z-coordinate rotation velocity at end timestamp expressed in
237 * radians per second (rad/s).
238 * @param dt time step expressed in seconds (t1 - t0).
239 * @param result instance where result of integration will be stored.
240 * @param quat initial attitude to be reused. Must be 4x1.
241 * @param omega0 angular speed at initial timestamp t0 to be reused. Must be 3x1.
242 * @param omega1 angular speed at end timestamp t1 to be reused. Must be 3x1.
243 * @param omegaSkew0 skew matrix of omega0 to be reused. Must be 4x4.
244 * @param omegaSkew1 skew matrix of omega1 to be reused. Must be 4x4.
245 * @param omegaAvgOmega average skew of angular speeds omega0 and omega1 to be reused. Must be 4x4.
246 * @param dotOmega derivative of angular speed to be reused. Must be 3x1.
247 * @param omegaSkewDotOmega skew matrix of derivative of angular speed to be reused. Must be 4x4.
248 * @param a temporary matrix to be reused. Must be 4x4.
249 * @param b temporary matrix to be reused. Must be 4x4.
250 * @param tmp temporary matrix to be reused. Must be 4x4.
251 * @param expA exponential matrix to be reused. Must be 4x4.
252 * @param quatResult instance where result of integration is stored in matrix form being
253 * reused. Must be 4x1.
254 * @throws RotationException if a numerical error occurs.
255 */
256 private static void integrationStep(
257 final Quaternion initialAttitude,
258 final double initialWx, final double initialWy, final double initialWz,
259 final double currentWx, final double currentWy, final double currentWz,
260 final double dt, final Quaternion result,
261 final ExponentialMatrixEstimator exponentialMatrixEstimator, final Matrix quat,
262 final Matrix omega0, final Matrix omega1, final Matrix omegaSkew0,
263 final Matrix omegaSkew1, final Matrix omegaAvgOmega, final Matrix dotOmega,
264 final Matrix omegaSkewDotOmega, final Matrix a, final Matrix b, final Matrix tmp,
265 final Matrix expA, final Matrix quatResult) throws RotationException {
266 try {
267 // normalize and copy initial attitude into matrix form
268 initialAttitude.normalize();
269 initialAttitude.values(quat.getBuffer());
270
271 // angular speed at initial timestamp t0
272 copyAngularSpeedToMatrix(initialWx, initialWy, initialWz, omega0);
273
274 // angular speed at end timestamp t1
275 copyAngularSpeedToMatrix(currentWx, currentWy, currentWz, omega1);
276
277 computeOmegaSkew(omega0, omegaSkew0);
278 computeOmegaSkew(omega1, omegaSkew1);
279
280 computeOmegaAvgOmega(omega0, omega1, dt, omegaSkew1, omegaAvgOmega, dotOmega, omegaSkewDotOmega);
281
282 a.copyFrom(omegaAvgOmega);
283 a.multiplyByScalar(0.5 * dt);
284
285 omegaSkew1.multiply(omegaSkew0, b);
286 omegaSkew0.multiply(omegaSkew1, tmp);
287 b.subtract(tmp);
288
289 exponentialMatrixEstimator.exponential(a, expA);
290
291 b.multiplyByScalar(dt * dt / 48.0);
292
293 tmp.copyFrom(expA);
294 tmp.add(b);
295
296 tmp.multiply(quat, quatResult);
297
298 result.setValues(quatResult.getBuffer());
299 result.normalize();
300 } catch (final AlgebraException e) {
301 throw new RotationException(e);
302 }
303 }
304
305 /**
306 * Computes the skew antisymmetric matrix of a vector matrix containing angular speed.
307 *
308 * @param omega0 column vector matrix containing angular speed. Must be 3x1.
309 * @param omega1 column vector matrix containing angular speed. Must be 3x1.
310 * @param dt time step expressed in seconds.
311 * @param omegaSkew1 instance being reused containing the skew antisymmetric matrix. Must be 4x4.
312 * @param result instance where result must be stored. Must be 4x4.
313 * @param dotOmega instance where computed time derivative will be stored. Must be 3x1.
314 * @param omegaSkewDotOmega instance where computed time derivative will be stored. Must be 4x4.
315 * @throws AlgebraException if provided matrices do not have proper size.
316 */
317 private static void computeOmegaAvgOmega(
318 final Matrix omega0, final Matrix omega1, final double dt, final Matrix omegaSkew1,
319 final Matrix result, final Matrix dotOmega, final Matrix omegaSkewDotOmega)
320 throws AlgebraException {
321 omega1.subtract(omega0, dotOmega);
322 dotOmega.multiplyByScalar(1.0 / dt);
323
324 computeOmegaSkew(dotOmega, omegaSkewDotOmega);
325
326 omegaSkewDotOmega.multiplyByScalar(0.5 * dt);
327
328 omegaSkew1.add(omegaSkewDotOmega, result);
329 }
330 }