1 /*
2 * Copyright (C) 2016 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.ar.slam;
17
18 import com.irurueta.algebra.ArrayUtils;
19 import com.irurueta.algebra.Matrix;
20 import com.irurueta.algebra.WrongSizeException;
21 import com.irurueta.geometry.Quaternion;
22 import com.irurueta.geometry.RotationUtils;
23
24 /**
25 * Utility class to predict rotations.
26 */
27 public class QuaternionPredictor {
28 /**
29 * Number of components on angular speed.
30 */
31 public static final int ANGULAR_SPEED_COMPONENTS = 3;
32
33 /**
34 * Constructor.
35 */
36 private QuaternionPredictor() {
37 }
38
39 /**
40 * Predicts the updated quaternion after a rotation in body frame expressed
41 * by the rate of rotation along axes x,y,z (roll, pitch, yaw).
42 *
43 * @param q quaternion to be updated.
44 * @param wx angular speed in x-axis (roll axis). Expressed in rad/s.
45 * @param wy angular speed in y-axis (pitch axis). Expressed in rad/s.
46 * @param wz angular speed in z-axis (yaw axis). Expressed in rad/s.
47 * @param dt time interval to compute prediction expressed in seconds.
48 * @param exactMethod true to use exact method, false to use "Tustin"
49 * method.
50 * @param result instance where updated quaternion is stored.
51 * @param jacobianQ jacobian wrt input quaternion. Must be 4x4.
52 * @param jacobianW jacobian wrt angular speed. Must be 4x3.
53 * @throws IllegalArgumentException if any of provided jacobians does not
54 * have proper size.
55 * @see <a href="https://github.com/joansola/slamtb">qpredict.m at https://github.com/joansola/slamtb</a>
56 */
57 public static void predict(
58 final Quaternion q, final double wx, final double wy, final double wz,
59 final double dt, final boolean exactMethod, final Quaternion result, final Matrix jacobianQ,
60 final Matrix jacobianW) {
61
62 if (jacobianQ != null && (jacobianQ.getRows() != Quaternion.N_PARAMS
63 || jacobianQ.getColumns() != Quaternion.N_PARAMS)) {
64 throw new IllegalArgumentException("jacobian wrt q must be 4x4");
65 }
66 if (jacobianW != null && (jacobianW.getRows() != Quaternion.N_PARAMS
67 || jacobianW.getColumns() != ANGULAR_SPEED_COMPONENTS)) {
68 throw new IllegalArgumentException("jacobian wrt w must be 4x3");
69 }
70
71 final var w = new double[]{wx, wy, wz};
72
73 if (exactMethod) {
74 // exact jacobians and rotation
75 ArrayUtils.multiplyByScalar(w, dt, w);
76 Quaternion.rotationVectorToQuaternion(w, result, jacobianW);
77 Matrix jacobianQ2 = null;
78 if (jacobianW != null) {
79 jacobianW.multiplyByScalar(dt);
80 try {
81 jacobianQ2 = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
82 } catch (final WrongSizeException ignore) {
83 // never happens
84 }
85 }
86 Quaternion.product(q, result, result, jacobianQ, jacobianQ2);
87
88 if (jacobianW != null && jacobianQ2 != null) {
89 try {
90 jacobianQ2.multiply(jacobianW);
91 jacobianW.copyFrom(jacobianQ2);
92 } catch (final WrongSizeException ignore) {
93 // never happens
94 }
95 }
96 } else {
97 // tustin integration - fits with jacobians
98 var skewW = RotationUtils.w2omega(w);
99 try {
100 final var qMatrix = new Matrix(Quaternion.N_PARAMS, 1);
101 qMatrix.setElementAtIndex(0, q.getA());
102 qMatrix.setElementAtIndex(1, q.getB());
103 qMatrix.setElementAtIndex(2, q.getC());
104 qMatrix.setElementAtIndex(3, q.getD());
105 skewW.multiply(qMatrix);
106 } catch (final WrongSizeException ignore) {
107 // never happens
108 }
109
110 skewW.multiplyByScalar(0.5 * dt);
111
112 result.setA(q.getA() + skewW.getElementAtIndex(0));
113 result.setB(q.getB() + skewW.getElementAtIndex(1));
114 result.setC(q.getC() + skewW.getElementAtIndex(2));
115 result.setD(q.getD() + skewW.getElementAtIndex(3));
116
117 if (jacobianQ != null) {
118 try {
119 // reset w values to reuse the same array as they might have been
120 // modified
121 w[0] = wx;
122 w[1] = wy;
123 w[2] = wz;
124
125 skewW = RotationUtils.w2omega(w);
126 jacobianQ.copyFrom(Matrix.identity(Quaternion.N_PARAMS, Quaternion.N_PARAMS));
127 jacobianQ.add(skewW);
128 jacobianQ.multiplyByScalar(0.5 * dt);
129 } catch (final WrongSizeException ignore) {
130 // never happens
131 }
132 }
133
134 if (jacobianW != null) {
135 RotationUtils.quaternionToConjugatedPiMatrix(q, jacobianW);
136 jacobianW.multiplyByScalar(0.5 * dt);
137 }
138 }
139 }
140
141 /**
142 * Predicts the updated quaternion after a rotation in body frame expressed
143 * by the rate of rotation along axes x, y, z (roll, pitch, yaw).
144 *
145 * @param q quaternion to be updated.
146 * @param w array containing angular speed in the 3 axis (x = roll,
147 * y = pitch, z = yaw). Expressed in rad/se. Must have length 3
148 * @param dt time interval to compute prediction expressed in seconds.
149 * @param exactMethod true to use exact method, false to use "Tustin"
150 * method.
151 * @param result instance where update quaternion is stored.
152 * @param jacobianQ jacobian wrt quaternion. Must be 4x4.
153 * @param jacobianW jacobian wrt angular speed. Must be 4x3.
154 * @throws IllegalArgumentException if any of provided jacobians does not
155 * have proper size, or w does not have length 3.
156 * @see <a href="https://github.com/joansola/slamtb">qpredict.m at https://github.com/joansola/slamtb</a>
157 */
158 public static void predict(
159 final Quaternion q, final double[] w, final double dt,
160 final boolean exactMethod, final Quaternion result, final Matrix jacobianQ, final Matrix jacobianW) {
161 if (w.length != ANGULAR_SPEED_COMPONENTS) {
162 throw new IllegalArgumentException("w must have length 3");
163 }
164 predict(q, w[0], w[1], w[2], dt, exactMethod, result, jacobianQ, jacobianW);
165 }
166
167 /**
168 * Predicts the updated quaternion after a rotation in body frame expressed
169 * by the rate of rotation along axes x, y, z (roll, pitch, yaw) using
170 * exact method.
171 *
172 * @param q quaternion to be updated.
173 * @param wx angular speed in x-axis (roll axis). Expressed in rad/s.
174 * @param wy angular speed in y-axis (pitch axis). Expressed in rad/s.
175 * @param wz angular speed in z-axis (yaw axis). Expressed in rad/s.
176 * @param dt time interval to compute prediction expressed in seconds.
177 * @param result instance where update quaternion is stored.
178 * @param jacobianQ jacobian wrt quaternion. Must be 4x4.
179 * @param jacobianW jacobian wrt angular speed. Must be 4x3.
180 * @throws IllegalArgumentException if any of provided jacobians does not
181 * have proper size.
182 * @see <a href="https://github.com/joansola/slamtb">qpredict.m at https://github.com/joansola/slamtb</a>
183 */
184 public static void predict(
185 final Quaternion q, final double wx, final double wy, final double wz, final double dt,
186 final Quaternion result, final Matrix jacobianQ, final Matrix jacobianW) {
187 predict(q, wx, wy, wz, dt, true, result, jacobianQ, jacobianW);
188 }
189
190 /**
191 * Predicts the updated quaternion after a rotation in body frame expressed
192 * by the rate of rotation along axes x, y, z (roll, pitch, yaw) using exact
193 * method.
194 *
195 * @param q quaternion to be updated.
196 * @param w array containing angular speed in the 3 axis (x = roll,
197 * y = pitch, z = yaw). Expressed in rad/se. Must have length 3
198 * @param dt time interval to compute prediction expressed in seconds.
199 * @param result instance where update quaternion is stored.
200 * @param jacobianQ jacobian wrt quaternion. Must be 4x4.
201 * @param jacobianW jacobian wrt angular speed. Must be 4x3.
202 * @throws IllegalArgumentException if any of provided jacobians does not
203 * have proper size.
204 * @see <a href="https://github.com/joansola/slamtb">qpredict.m at https://github.com/joansola/slamtb</a>
205 */
206 public static void predict(
207 final Quaternion q, final double[] w, final double dt, final Quaternion result, final Matrix jacobianQ,
208 final Matrix jacobianW) {
209 predict(q, w, dt, true, result, jacobianQ, jacobianW);
210 }
211
212 /**
213 * Predicts the updated quaternion after a rotation in body frame expressed
214 * by the rate of rotation along axes x, y, z (roll, pitch, yaw).
215 *
216 * @param q quaternion to be updated.
217 * @param wx angular speed in x-axis (roll axis). Expressed in rad/s.
218 * @param wy angular speed in y-axis (pitch axis). Expressed in rad/s.
219 * @param wz angular speed in z-axis (yaw axis). Expressed in rad/s.
220 * @param dt time interval to compute prediction expressed in seconds.
221 * @param exactMethod true to use exact method, false to use "Tustin"
222 * method.
223 * @param result instance where update quaternion is stored.
224 * @see <a href="https://github.com/joansola/slamtb">qpredict.m at https://github.com/joansola/slamtb</a>
225 */
226 public static void predict(
227 final Quaternion q, final double wx, final double wy, final double wz, final double dt,
228 final boolean exactMethod, final Quaternion result) {
229 predict(q, wx, wy, wz, dt, exactMethod, result, null, null);
230 }
231
232 /**
233 * Predicts the updated quaternion after a rotation in body frame expressed
234 * by the rate of rotation along axes x, y, z (roll, pitch, yaw).
235 *
236 * @param q quaternion to be updated.
237 * @param w array containing angular speed in the 3 axis (x = roll,
238 * y = pitch, z = yaw). Expressed in rad/se. Must have length 3
239 * @param dt time interval to compute prediction expressed in seconds.
240 * @param exactMethod true to use exact method, false to use "Tustin"
241 * method.
242 * @param result instance where update quaternion is stored.
243 * @throws IllegalArgumentException if w does not have length 3.
244 * @see <a href="https://github.com/joansola/slamtb">qpredict.m at https://github.com/joansola/slamtb</a>
245 */
246 public static void predict(
247 final Quaternion q, final double[] w, final double dt, final boolean exactMethod, final Quaternion result) {
248 predict(q, w, dt, exactMethod, result, null, null);
249 }
250
251 /**
252 * Predicts the updated quaternion after a rotation in body frame expressed
253 * by the rate of rotation along axes x, y, z (roll, pitch, yaw) using exact
254 * method.
255 *
256 * @param q quaternion to be updated.
257 * @param wx angular speed in x-axis (roll axis). Expressed in rad/s.
258 * @param wy angular speed in y-axis (pitch axis). Expressed in rad/s.
259 * @param wz angular speed in z-axis (yaw axis). Expressed in rad/s.
260 * @param dt time interval to compute prediction expressed in seconds.
261 * @param result instance where update quaternion is stored.
262 * @see <a href="https://github.com/joansola/slamtb">qpredict.m at https://github.com/joansola/slamtb</a>
263 */
264 public static void predict(
265 final Quaternion q, final double wx, final double wy, final double wz, final double dt,
266 final Quaternion result) {
267 predict(q, wx, wy, wz, dt, result, null, null);
268 }
269
270 /**
271 * Predicts the updated quaternion after a rotation in body frame expressed
272 * by the rate of rotation along axes x, y, z (roll, pitch, yaw) using exact
273 * method.
274 *
275 * @param q quaternion to be updated.
276 * @param w array containing angular speed in the 3 axis (x = roll,
277 * y = pitch, z = yaw). Expressed in rad/se. Must have length 3
278 * @param dt time interval to compute prediction expressed in seconds.
279 * @param result instance where update quaternion is stored.
280 * @throws IllegalArgumentException if w does not have length 3
281 * @see <a href="https://github.com/joansola/slamtb">qpredict.m at https://github.com/joansola/slamtb</a>
282 */
283 public static void predict(
284 final Quaternion q, final double[] w, final double dt, final Quaternion result) {
285 predict(q, w, dt, result, null, null);
286 }
287
288 /**
289 * Predicts the updated quaternion after a rotation in body frame expressed
290 * by the rate of rotation along axes x, y, z (roll, pitch, yaw).
291 *
292 * @param q quaternion to be updated.
293 * @param wx angular speed in x-axis (roll axis). Expressed in rad/s.
294 * @param wy angular speed in y-axis (pitch axis). Expressed in rad/s.
295 * @param wz angular speed in z-axis (yaw axis). Expressed in rad/s.
296 * @param dt time interval to compute prediction expressed in seconds.
297 * @param exactMethod true to use exact method, false to use "Tustin"
298 * method.
299 * @param jacobianQ jacobian wrt quaternion. Must be 4x4.
300 * @param jacobianW jacobian wrt angular speed. Must be 4x3.
301 * @return a new quaternion containing updated quaternion.
302 * @throws IllegalArgumentException if any of provided jacobians does not
303 * have proper size.
304 * @see <a href="https://github.com/joansola/slamtb">qpredict.m at https://github.com/joansola/slamtb</a>
305 */
306 public static Quaternion predict(
307 final Quaternion q, final double wx, final double wy, final double wz, final double dt,
308 final boolean exactMethod, final Matrix jacobianQ, final Matrix jacobianW) {
309 final var result = new Quaternion();
310 predict(q, wx, wy, wz, dt, exactMethod, result, jacobianQ, jacobianW);
311 return result;
312 }
313
314 /**
315 * Predicts the updated quaternion after a rotation in body frame expressed
316 * by the rate of rotation along axes x, y, z (roll, pitch, yaw).
317 *
318 * @param q quaternion to be updated.
319 * @param w array containing angular speed in the 3 axis (x = roll,
320 * y = pitch, z = yaw). Expressed in rad/se. Must have length 3
321 * @param dt time interval to compute prediction expressed in seconds.
322 * @param exactMethod true to use exact method, false to use "Tustin"
323 * method.
324 * @param jacobianQ jacobian wrt quaternion. Must be 4x4.
325 * @param jacobianW jacobian wrt angular speed. Must be 4x3.
326 * @return a new quaternion containing updated quaternion.
327 * @throws IllegalArgumentException if any of provided jacobians does not
328 * have proper size or w does not have length 3.
329 * @see <a href="https://github.com/joansola/slamtb">qpredict.m at https://github.com/joansola/slamtb</a>
330 */
331 public static Quaternion predict(
332 final Quaternion q, final double[] w, final double dt, final boolean exactMethod, final Matrix jacobianQ,
333 final Matrix jacobianW) {
334 final var result = new Quaternion();
335 predict(q, w, dt, exactMethod, result, jacobianQ, jacobianW);
336 return result;
337 }
338
339 /**
340 * Predicts the updated quaternion after a rotation in body frame expressed
341 * by the rate of rotation along axes x, y, z (roll, pitch, yaw) using exact
342 * method.
343 *
344 * @param q quaternion to be updated.
345 * @param wx angular speed in x-axis (roll axis). Expressed in rad/s.
346 * @param wy angular speed in y-axis (pitch axis). Expressed in rad/s.
347 * @param wz angular speed in z-axis (yaw axis). Expressed in rad/s.
348 * @param dt time interval to compute prediction expressed in seconds.
349 * @param jacobianQ jacobian wrt quaternion. Must be 4x4.
350 * @param jacobianW jacobian wrt angular speed. Must be 4x3.
351 * @return a new quaternion containing updated quaternion.
352 * @throws IllegalArgumentException if any of provided jacobians does not
353 * have proper size.
354 * @see <a href="https://github.com/joansola/slamtb">qpredict.m at https://github.com/joansola/slamtb</a>
355 */
356 public static Quaternion predict(
357 final Quaternion q, final double wx, final double wy, final double wz, final double dt,
358 final Matrix jacobianQ, final Matrix jacobianW) {
359 final var result = new Quaternion();
360 predict(q, wx, wy, wz, dt, result, jacobianQ, jacobianW);
361 return result;
362 }
363
364 /**
365 * Predicts the updated quaternion after a rotation in body frame expressed
366 * by the rate of rotation along axes x, y, z (roll, pitch, yaw) using exact
367 * method.
368 *
369 * @param q quaternion to be updated.
370 * @param w array containing angular speed in the 3 axis (x = roll,
371 * y = pitch, z = yaw). Expressed in rad/se. Must have length 3
372 * @param dt time interval to compute prediction expressed in seconds.
373 * @param jacobianQ jacobian wrt quaternion. Must be 4x4.
374 * @param jacobianW jacobian wrt angular speed. Must be 4x3.
375 * @return a new quaternion containing updated quaternion.
376 * @throws IllegalArgumentException if any of provided jacobians does not
377 * have proper size.
378 * @see <a href="https://github.com/joansola/slamtb">qpredict.m at https://github.com/joansola/slamtb</a>
379 */
380 public static Quaternion predict(
381 final Quaternion q, final double[] w, final double dt, final Matrix jacobianQ, final Matrix jacobianW) {
382 final var result = new Quaternion();
383 predict(q, w, dt, result, jacobianQ, jacobianW);
384 return result;
385 }
386
387 /**
388 * Predicts the updated quaternion after a rotation in body frame expressed
389 * by the rate of rotation along axes x, y, z (roll, pitch, yaw).
390 *
391 * @param q quaternion to be updated.
392 * @param wx angular speed in x-axis (roll axis). Expressed in rad/s.
393 * @param wy angular speed in y-axis (pitch axis). Expressed in rad/s.
394 * @param wz angular speed in z-axis (yaw axis). Expressed in rad/s.
395 * @param dt time interval to compute prediction expressed in seconds.
396 * @param exactMethod true to use exact method, false to use "Tustin"
397 * method.
398 * @return a new quaternion containing updated quaternion.
399 * @see <a href="https://github.com/joansola/slamtb">qpredict.m at https://github.com/joansola/slamtb</a>
400 */
401 public static Quaternion predict(
402 final Quaternion q, final double wx, final double wy, final double wz, final double dt,
403 final boolean exactMethod) {
404 final var result = new Quaternion();
405 predict(q, wx, wy, wz, dt, exactMethod, result);
406 return result;
407 }
408
409 /**
410 * Predicts the updated quaternion after a rotation in body frame expressed
411 * by the rate of rotation along axes x, y, z (roll, pitch, yaw).
412 *
413 * @param q quaternion to be updated.
414 * @param w array containing angular speed in the 3 axis (x = roll,
415 * y = pitch, z = yaw). Expressed in rad/se. Must have length 3
416 * @param dt time interval to compute prediction expressed in seconds.
417 * @param exactMethod true to use exact method, false to use "Tustin"
418 * method.
419 * @return a new quaternion containing updated quaternion.
420 * @throws IllegalArgumentException if w does not have length 3
421 * @see <a href="https://github.com/joansola/slamtb">qpredict.m at https://github.com/joansola/slamtb</a>
422 */
423 public static Quaternion predict(final Quaternion q, final double[] w, final double dt, final boolean exactMethod) {
424 final var result = new Quaternion();
425 predict(q, w, dt, exactMethod, result);
426 return result;
427 }
428
429 /**
430 * Predicts the updated quaternion after a rotation in body frame expressed
431 * by the rate of rotation along axes x, y, z (roll, pitch, yaw) using exact
432 * method.
433 *
434 * @param q quaternion to be updated.
435 * @param wx angular speed in x-axis (roll axis). Expressed in rad/s.
436 * @param wy angular speed in y-axis (pitch axis). Expressed in rad/s.
437 * @param wz angular speed in z-axis (yaw axis). Expressed in rad/s.
438 * @param dt time interval to compute prediction expressed in seconds.
439 * @return a new quaternion containing updated quaternion.
440 * @see <a href="https://github.com/joansola/slamtb">qpredict.m at https://github.com/joansola/slamtb</a>
441 */
442 public static Quaternion predict(final Quaternion q, final double wx, final double wy, final double wz,
443 final double dt) {
444 final var result = new Quaternion();
445 predict(q, wx, wy, wz, dt, result);
446 return result;
447 }
448
449 /**
450 * Predicts the updated quaternion after a rotation in body frame expressed
451 * by the rate of rotation along axes x, y, z (roll, pitch, yaw) using exact
452 * method.
453 *
454 * @param q quaternion to be updated.
455 * @param w array containing angular speed in the 3 axis (x = roll,
456 * y = pitch, z = yaw). Expressed in rad/se. Must have length 3
457 * @param dt time interval to compute prediction expressed in seconds.
458 * @return a new quaternion containing updated quaternion.
459 * @throws IllegalArgumentException if w does not have length 3.
460 * @see <a href="https://github.com/joansola/slamtb">qpredict.m at https://github.com/joansola/slamtb</a>
461 */
462 public static Quaternion predict(final Quaternion q, final double[] w, final double dt) {
463 final var result = new Quaternion();
464 predict(q, w, dt, result);
465 return result;
466 }
467
468 /**
469 * Predicts the updated quaternion after a rotation in body frame expressed
470 * by the rate of rotation along axes x, y, z (roll, pitch, yaw) assuming a
471 * time interval of 1 second.
472 *
473 * @param q quaternion to be updated.
474 * @param wx angular speed in x-axis (roll axis). Expressed in rad/s.
475 * @param wy angular speed in y-axis (pitch axis). Expressed in rad/s.
476 * @param wz angular speed in z-axis (yaw axis). Expressed in rad/s.
477 * @param exactMethod true to use exact method, false to use "Tustin"
478 * method.
479 * @param result instance where update quaternion is stored.
480 * @param jacobianQ jacobian wrt quaternion. Must be 4x4.
481 * @param jacobianW jacobian wrt angular speed. Must be 4x3.
482 * @throws IllegalArgumentException if any of provided jacobians does not
483 * have proper size.
484 * @see <a href="https://github.com/joansola/slamtb">qpredict.m at https://github.com/joansola/slamtb</a>
485 */
486 public static void predict(
487 final Quaternion q, final double wx, final double wy, final double wz, final boolean exactMethod,
488 final Quaternion result, final Matrix jacobianQ, final Matrix jacobianW) {
489 predict(q, wx, wy, wz, 1.0, exactMethod, result, jacobianQ, jacobianW);
490 }
491
492 /**
493 * Predicts the updated quaternion after a rotation in body frame expressed
494 * by the rate of rotation along axes x, y, z (roll, pitch, yaw) assuming a
495 * time interval of 1 second.
496 *
497 * @param q quaternion to be updated.
498 * @param w array containing angular speed in the 3 axis (x = roll,
499 * y = pitch, z = yaw). Expressed in rad/se. Must have length 3
500 * @param exactMethod true to use exact method, false to use "Tustin"
501 * method.
502 * @param result instance where update quaternion is stored.
503 * @param jacobianQ jacobian wrt quaternion. Must be 4x4.
504 * @param jacobianW jacobian wrt angular speed. Must be 4x3.
505 * @throws IllegalArgumentException if any of provided jacobians does not
506 * have proper size.
507 * @see <a href="https://github.com/joansola/slamtb">qpredict.m at https://github.com/joansola/slamtb</a>
508 */
509 public static void predict(
510 final Quaternion q, final double[] w, final boolean exactMethod, final Quaternion result,
511 final Matrix jacobianQ, final Matrix jacobianW) {
512 predict(q, w, 1.0, exactMethod, result, jacobianQ, jacobianW);
513 }
514
515 /**
516 * Predicts the updated quaternion after a rotation in body frame expressed
517 * by the rate of rotation along axes x, y, z (roll, pitch, yaw) assuming a
518 * time interval of 1 second and exact method.
519 *
520 * @param q quaternion to be updated.
521 * @param wx angular speed in x-axis (roll axis). Expressed in rad/s.
522 * @param wy angular speed in y-axis (pitch axis). Expressed in rad/s.
523 * @param wz angular speed in z-axis (yaw axis). Expressed in rad/s.
524 * @param result instance where update quaternion is stored.
525 * @param jacobianQ jacobian wrt quaternion. Must be 4x4.
526 * @param jacobianW jacobian wrt angular speed. Must be 4x3.
527 * @throws IllegalArgumentException if any of provided jacobians does not
528 * have proper size.
529 * @see <a href="https://github.com/joansola/slamtb">qpredict.m at https://github.com/joansola/slamtb</a>
530 */
531 public static void predict(
532 final Quaternion q, final double wx, final double wy, final double wz, final Quaternion result,
533 final Matrix jacobianQ, final Matrix jacobianW) {
534 predict(q, wx, wy, wz, 1.0, result, jacobianQ, jacobianW);
535 }
536
537 /**
538 * Predicts the updated quaternion after a rotation in body frame expressed
539 * by the rate of rotation along axes x, y, z (roll, pitch, yaw) assuming a
540 * time interval of 1 second and exact method.
541 *
542 * @param q quaternion to be updated.
543 * @param w array containing angular speed in the 3 axis (x = roll,
544 * y = pitch, z = yaw). Expressed in rad/se. Must have length 3
545 * @param result instance where update quaternion is stored.
546 * @param jacobianQ jacobian wrt quaternion. Must be 4x4.
547 * @param jacobianW jacobian wrt angular speed. Must be 4x3.
548 * @throws IllegalArgumentException if any of provided jacobians does not
549 * have proper size.
550 * @see <a href="https://github.com/joansola/slamtb">qpredict.m at https://github.com/joansola/slamtb</a>
551 */
552 public static void predict(
553 final Quaternion q, final double[] w, final Quaternion result, final Matrix jacobianQ,
554 final Matrix jacobianW) {
555 predict(q, w, 1.0, result, jacobianQ, jacobianW);
556 }
557
558 /**
559 * Predicts the updated quaternion after a rotation in body frame expressed
560 * by the rate of rotation along axes x, y, z (roll, pitch, yaw) assuming a
561 * time interval of 1 second.
562 *
563 * @param q quaternion to be updated.
564 * @param wx angular speed in x-axis (roll axis). Expressed in rad/s.
565 * @param wy angular speed in y-axis (pitch axis). Expressed in rad/s.
566 * @param wz angular speed in z-axis (yaw axis). Expressed in rad/s.
567 * @param exactMethod true to use exact method, false to use "Tustin"
568 * method.
569 * @param result instance where update quaternion is stored.
570 * @see <a href="https://github.com/joansola/slamtb">qpredict.m at https://github.com/joansola/slamtb</a>
571 */
572 public static void predict(
573 final Quaternion q, final double wx, final double wy, final double wz, final boolean exactMethod,
574 final Quaternion result) {
575 predict(q, wx, wy, wz, 1.0, exactMethod, result);
576 }
577
578 /**
579 * Predicts the updated quaternion after a rotation in body frame expressed
580 * by the rate of rotation along axes x, y, z (roll, pitch, yaw) assuming a
581 * time interval of 1 second.
582 *
583 * @param q quaternion to be updated.
584 * @param w array containing angular speed in the 3 axis (x = roll,
585 * y = pitch, z = yaw). Expressed in rad/se. Must have length 3
586 * @param exactMethod true to use exact method, false to use "Tustin"
587 * method.
588 * @param result instance where update quaternion is stored.
589 * @throws IllegalArgumentException if any of provided jacobians does not
590 * have proper size.
591 * @see <a href="https://github.com/joansola/slamtb">qpredict.m at https://github.com/joansola/slamtb</a>
592 */
593 public static void predict(
594 final Quaternion q, final double[] w, final boolean exactMethod, final Quaternion result) {
595 predict(q, w, 1.0, exactMethod, result);
596 }
597
598 /**
599 * Predicts the updated quaternion after a rotation in body frame expressed
600 * by the rate of rotation along axes x, y, z (roll, pitch, yaw) assuming a
601 * time interval of 1 second and exact method.
602 *
603 * @param q quaternion to be updated.
604 * @param wx angular speed in x-axis (roll axis). Expressed in rad/s.
605 * @param wy angular speed in y-axis (pitch axis). Expressed in rad/s.
606 * @param wz angular speed in z-axis (yaw axis). Expressed in rad/s.
607 * @param result instance where update quaternion is stored.
608 * @see <a href="https://github.com/joansola/slamtb">qpredict.m at https://github.com/joansola/slamtb</a>
609 */
610 public static void predict(
611 final Quaternion q, final double wx, final double wy, final double wz, final Quaternion result) {
612 predict(q, wx, wy, wz, 1.0, result);
613 }
614
615 /**
616 * Predicts the updated quaternion after a rotation in body frame expressed
617 * by the rate of rotation along axes x, y, z (roll, pitch, yaw) assuming a
618 * time interval of 1 second and exact method.
619 *
620 * @param q quaternion to be updated.
621 * @param w array containing angular speed in the 3 axis (x = roll,
622 * y = pitch, z = yaw). Expressed in rad/se. Must have length 3
623 * @param result instance where update quaternion is stored.
624 * @throws IllegalArgumentException if any of provided jacobians does not
625 * have proper size.
626 * @see <a href="https://github.com/joansola/slamtb">qpredict.m at https://github.com/joansola/slamtb</a>
627 */
628 public static void predict(final Quaternion q, final double[] w, final Quaternion result) {
629 predict(q, w, 1.0, result);
630 }
631
632 /**
633 * Predicts the updated quaternion after a rotation in body frame expressed
634 * by the rate of rotation along axes x, y, z (roll, pitch, yaw) assuming a
635 * time interval of 1 second.
636 *
637 * @param q quaternion to be updated.
638 * @param wx angular speed in x-axis (roll axis). Expressed in rad/s.
639 * @param wy angular speed in y-axis (pitch axis). Expressed in rad/s.
640 * @param wz angular speed in z-axis (yaw axis). Expressed in rad/s.
641 * @param exactMethod true to use exact method, false to use "Tustin"
642 * method.
643 * @param jacobianQ jacobian wrt quaternion. Must be 4x4.
644 * @param jacobianW jacobian wrt angular speed. Must be 4x3.
645 * @return a new quaternion containing updated quaternion.
646 * @throws IllegalArgumentException if any of provided jacobians does not
647 * have proper size.
648 * @see <a href="https://github.com/joansola/slamtb">qpredict.m at https://github.com/joansola/slamtb</a>
649 */
650 public static Quaternion predict(
651 final Quaternion q, final double wx, final double wy, final double wz, final boolean exactMethod,
652 final Matrix jacobianQ, final Matrix jacobianW) {
653 return predict(q, wx, wy, wz, 1.0, exactMethod, jacobianQ, jacobianW);
654 }
655
656 /**
657 * Predicts the updated quaternion after a rotation in body frame expressed
658 * by the rate of rotation along axes x, y, z (roll, pitch, yaw) assuming a
659 * time interval of 1 second.
660 *
661 * @param q quaternion to be updated.
662 * @param w array containing angular speed in the 3 axis (x = roll,
663 * y = pitch, z = yaw). Expressed in rad/se. Must have length 3
664 * @param exactMethod true to use exact method, false to use "Tustin"
665 * method.
666 * @param jacobianQ jacobian wrt quaternion. Must be 4x4.
667 * @param jacobianW jacobian wrt angular speed. Must be 4x3.
668 * @return a new quaternion containing updated quaternion.
669 * @throws IllegalArgumentException if any of provided jacobians does not
670 * have proper size.
671 * @see <a href="https://github.com/joansola/slamtb">qpredict.m at https://github.com/joansola/slamtb</a>
672 */
673 public static Quaternion predict(
674 final Quaternion q, final double[] w, final boolean exactMethod, final Matrix jacobianQ,
675 final Matrix jacobianW) {
676 return predict(q, w, 1.0, exactMethod, jacobianQ, jacobianW);
677 }
678
679 /**
680 * Predicts the updated quaternion after a rotation in body frame expressed
681 * by the rate of rotation along axes x, y, z (roll, pitch, yaw) assuming a
682 * time interval of 1 second and exact method.
683 *
684 * @param q quaternion to be updated.
685 * @param wx angular speed in x-axis (roll axis). Expressed in rad/s.
686 * @param wy angular speed in y-axis (pitch axis). Expressed in rad/s.
687 * @param wz angular speed in z-axis (yaw axis). Expressed in rad/s.
688 * @param jacobianQ jacobian wrt quaternion. Must be 4x4.
689 * @param jacobianW jacobian wrt angular speed. Must be 4x3.
690 * @return a new quaternion containing updated quaternion.
691 * @throws IllegalArgumentException if any of provided jacobians does not
692 * have proper size.
693 * @see <a href="https://github.com/joansola/slamtb">qpredict.m at https://github.com/joansola/slamtb</a>
694 */
695 public static Quaternion predict(
696 final Quaternion q, final double wx, final double wy, final double wz, final Matrix jacobianQ,
697 final Matrix jacobianW) {
698 return predict(q, wx, wy, wz, 1.0, jacobianQ, jacobianW);
699 }
700
701 /**
702 * Predicts the updated quaternion after a rotation in body frame expressed
703 * by the rate of rotation along axes x, y, z (roll, pitch, yaw) assuming a
704 * time interval of 1 second and exact method.
705 *
706 * @param q quaternion to be updated.
707 * @param w array containing angular speed in the 3 axis (x = roll,
708 * y = pitch, z = yaw). Expressed in rad/se. Must have length 3
709 * @param jacobianQ jacobian wrt quaternion. Must be 4x4.
710 * @param jacobianW jacobian wrt angular speed. Must be 4x3.
711 * @return a new quaternion containing updated quaternion.
712 * @throws IllegalArgumentException if any of provided jacobians does not
713 * have proper size.
714 * @see <a href="https://github.com/joansola/slamtb">qpredict.m at https://github.com/joansola/slamtb</a>
715 */
716 public static Quaternion predict(final Quaternion q, final double[] w, final Matrix jacobianQ,
717 final Matrix jacobianW) {
718 return predict(q, w, 1.0, jacobianQ, jacobianW);
719 }
720
721 /**
722 * Predicts the updated quaternion after a rotation in body frame expressed
723 * by the rate of rotation along axes x, y, z (roll, pitch, yaw) assuming a
724 * time interval of 1 second.
725 *
726 * @param q quaternion to be updated.
727 * @param wx angular speed in x-axis (roll axis). Expressed in rad/s.
728 * @param wy angular speed in y-axis (pitch axis). Expressed in rad/s.
729 * @param wz angular speed in z-axis (yaw axis). Expressed in rad/s.
730 * @param exactMethod true to use exact method, false to use "Tustin"
731 * method.
732 * @return a new quaternion containing updated quaternion.
733 * @see <a href="https://github.com/joansola/slamtb">qpredict.m at https://github.com/joansola/slamtb</a>
734 */
735 public static Quaternion predict(
736 final Quaternion q, final double wx, final double wy, final double wz, final boolean exactMethod) {
737 return predict(q, wx, wy, wz, 1.0, exactMethod);
738 }
739
740 /**
741 * Predicts the updated quaternion after a rotation in body frame expressed
742 * by the rate of rotation along axes x, y, z (roll, pitch, yaw) assuming a
743 * time interval of 1 second.
744 *
745 * @param q quaternion to be updated.
746 * @param w array containing angular speed in the 3 axis (x = roll,
747 * y = pitch, z = yaw). Expressed in rad/se. Must have length 3
748 * @param exactMethod true to use exact method, false to use "Tustin"
749 * method.
750 * @return a new quaternion containing updated quaternion.
751 * @see <a href="https://github.com/joansola/slamtb">qpredict.m at https://github.com/joansola/slamtb</a>
752 */
753 public static Quaternion predict(final Quaternion q, final double[] w, final boolean exactMethod) {
754 return predict(q, w, 1.0, exactMethod);
755 }
756
757 /**
758 * Predicts the updated quaternion after a rotation in body frame expressed
759 * by the rate of rotation along axes x, y, z (roll, pitch, yaw) assuming a
760 * time interval of 1 second and exact method.
761 *
762 * @param q quaternion to be updated.
763 * @param wx angular speed in x-axis (roll axis). Expressed in rad/s.
764 * @param wy angular speed in y-axis (pitch axis). Expressed in rad/s.
765 * @param wz angular speed in z-axis (yaw axis). Expressed in rad/s.
766 * @return a new quaternion containing updated quaternion.
767 * @see <a href="https://github.com/joansola/slamtb">qpredict.m at https://github.com/joansola/slamtb</a>
768 */
769 public static Quaternion predict(final Quaternion q, final double wx, final double wy, final double wz) {
770 return predict(q, wx, wy, wz, 1.0);
771 }
772
773 /**
774 * Predicts the updated quaternion after a rotation in body frame expressed
775 * by the rate of rotation along axes x, y, z (roll, pitch, yaw) assuming a
776 * time interval of 1 second and exact method.
777 *
778 * @param q quaternion to be updated.
779 * @param w array containing angular speed in the 3 axis (x = roll,
780 * y = pitch, z = yaw). Expressed in rad/se. Must have length 3
781 * @return a new quaternion containing updated quaternion.
782 * @throws IllegalArgumentException if any of provided jacobians does not
783 * have proper size.
784 * @see <a href="https://github.com/joansola/slamtb">qpredict.m at https://github.com/joansola/slamtb</a>
785 */
786 public static Quaternion predict(final Quaternion q, final double[] w) {
787 return predict(q, w, 1.0);
788 }
789
790 /**
791 * Predicts the updated quaternion after a rotation in body frame expressed
792 * by provided rotation dq and by provided rate of rotation along axes
793 * x,y,z (roll, pitch, yaw).
794 *
795 * @param q quaternion to be updated.
796 * @param dq adjustment of rotation to be combined with input quaternion.
797 * @param wx angular speed in x-axis (roll axis). Expressed in rad/s.
798 * @param wy angular speed in y-axis (pitch axis). Expressed in rad/s.
799 * @param wz angular speed in z-axis (yaw axis). Expressed in rad/s.
800 * @param dt time interval to compute prediction expressed in seconds.
801 * @param result instance where updated quaternion is stored.
802 * @param jacobianQ jacobian wrt input quaternion. Must be 4x4.
803 * @param jacobianDQ jacobian wrt dq quaternion. Must be 4x4.
804 * @param jacobianW jacobian wrt angular speed. Must be 4x3.
805 * @throws IllegalArgumentException if any of provided jacobians does not
806 * have proper size.
807 */
808 public static void predictWithRotationAdjustment(
809 final Quaternion q, final Quaternion dq, final double wx, final double wy, final double wz, final double dt,
810 final Quaternion result, final Matrix jacobianQ, final Matrix jacobianDQ, final Matrix jacobianW) {
811
812 if (jacobianQ != null && (jacobianQ.getRows() != Quaternion.N_PARAMS
813 || jacobianQ.getColumns() != Quaternion.N_PARAMS)) {
814 throw new IllegalArgumentException("jacobian wrt q must be 4x4");
815 }
816 if (jacobianDQ != null && (jacobianDQ.getRows() != Quaternion.N_PARAMS
817 || jacobianDQ.getColumns() != Quaternion.N_PARAMS)) {
818 throw new IllegalArgumentException("jacobian wrt dq must be 4x4");
819 }
820 if (jacobianW != null && (jacobianW.getRows() != Quaternion.N_PARAMS
821 || jacobianW.getColumns() != ANGULAR_SPEED_COMPONENTS)) {
822 throw new IllegalArgumentException("jacobian wrt w must be 4x3");
823 }
824
825 final var w = new double[]{wx, wy, wz};
826
827 ArrayUtils.multiplyByScalar(w, dt, w);
828 Quaternion.rotationVectorToQuaternion(w, result, jacobianW);
829 Matrix jacobianQ2 = null;
830 if (jacobianW != null) {
831 jacobianW.multiplyByScalar(dt);
832 }
833 if (jacobianW != null) {
834 try {
835 jacobianQ2 = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
836 } catch (final WrongSizeException ignore) {
837 // never happens
838 }
839 }
840 Quaternion.product(dq, result, result, jacobianDQ, jacobianQ2);
841
842 Matrix jacobianQ3 = null;
843 if (jacobianDQ != null || jacobianW != null) {
844 try {
845 jacobianQ3 = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
846 } catch (final WrongSizeException ignore) {
847 // never happens
848 }
849 }
850 Quaternion.product(q, result, result, jacobianQ, jacobianQ3);
851
852 // chain rule
853 if (jacobianQ3 != null) {
854 if (jacobianDQ != null) {
855 try {
856 final var tmp = jacobianQ3.multiplyAndReturnNew(jacobianDQ);
857 jacobianDQ.copyFrom(tmp);
858 } catch (final WrongSizeException ignore) {
859 // never happens
860 }
861 }
862
863 // chain rule (jacobianW is already multiplied by dt)
864 if (jacobianW != null && jacobianQ2 != null) {
865 try {
866 jacobianQ3.multiply(jacobianQ2);
867 jacobianQ3.multiply(jacobianW);
868 jacobianW.copyFrom(jacobianQ3);
869 } catch (final WrongSizeException ignore) {
870 // never happens
871 }
872 }
873 }
874 }
875
876 /**
877 * Predicts the updated quaternion after a rotation in body frame expressed
878 * by provided rotation dq and by provided rate of rotation along axes x, y,
879 * z (roll, pitch, yaw).
880 *
881 * @param q quaternion to be updated.
882 * @param dq adjustment of rotation to be combined with input quaternion.
883 * @param w angular speed (x, y, z axes). Expressed in rad/s. Must have
884 * length 3.
885 * @param dt time interval to compute prediction expressed in seconds.
886 * @param result instance where updated quaternion is stored.
887 * @param jacobianQ jacobian wrt input quaternion. Must be 4x4.
888 * @param jacobianDQ jacobian wrt dq quaternion. Must be 4x4.
889 * @param jacobianW jacobian wrt angular speed. Must be 4x3.
890 * @throws IllegalArgumentException if any of provided jacobians does not
891 * have proper size or if w does not have length 3.
892 */
893 public static void predictWithRotationAdjustment(
894 final Quaternion q, final Quaternion dq, final double[] w, final double dt, final Quaternion result,
895 final Matrix jacobianQ, final Matrix jacobianDQ, final Matrix jacobianW) {
896 if (w.length != ANGULAR_SPEED_COMPONENTS) {
897 throw new IllegalArgumentException("w must have length 3");
898 }
899 predictWithRotationAdjustment(q, dq, w[0], w[1], w[2], dt, result, jacobianQ, jacobianDQ, jacobianW);
900 }
901
902 /**
903 * Predicts the updated quaternion after a rotation in body frame expressed
904 * by provided rotation dq and by provided rate of rotation along axes
905 * x, y, z (roll, pitch, yaw).
906 *
907 * @param q quaternion to be updated.
908 * @param dq adjustment of rotation to be combined with input quaternion.
909 * @param wx angular speed in x-axis (roll axis). Expressed in rad/s.
910 * @param wy angular speed in y-axis (pitch axis). Expressed in rad/s.
911 * @param wz angular speed in z-axis (yaw axis). Expressed in rad/s.
912 * @param dt time interval to compute prediction expressed in seconds.
913 * @param result instance where updated quaternion is stored.
914 */
915 public static void predictWithRotationAdjustment(
916 final Quaternion q, final Quaternion dq, final double wx, final double wy, final double wz, final double dt,
917 final Quaternion result) {
918 predictWithRotationAdjustment(q, dq, wx, wy, wz, dt, result, null, null, null);
919 }
920
921 /**
922 * Predicts the updated quaternion after a rotation in body frame expressed
923 * by provided rotation dq and by provided rate of rotation along axes x,y,z
924 * (roll, pitch, yaw).
925 *
926 * @param q quaternion to be updated.
927 * @param dq adjustment of rotation to be combined with input quaternion.
928 * @param w angular speed (x, y, z axes). Expressed in rad/s. Must have
929 * length 3.
930 * @param dt time interval to compute prediction expressed in seconds.
931 * @param result instance where updated quaternion is stored.
932 * @throws IllegalArgumentException if w does not have length 3.
933 */
934 public static void predictWithRotationAdjustment(
935 final Quaternion q, final Quaternion dq, final double[] w, final double dt, final Quaternion result) {
936 predictWithRotationAdjustment(q, dq, w, dt, result, null, null, null);
937 }
938
939 /**
940 * Predicts the updated quaternion after a rotation in body frame expressed
941 * by provided rotation dq and by provided rate of rotation along axes x,y,z
942 * (roll, pitch, yaw).
943 *
944 * @param q quaternion to be updated.
945 * @param dq adjustment of rotation to be combined with input quaternion.
946 * @param wx angular speed in x-axis (roll axis). Expressed in rad/s.
947 * @param wy angular speed in y-axis (pitch axis). Expressed in rad/s.
948 * @param wz angular speed in z-axis (yaw axis). Expressed in rad/s.
949 * @param dt time interval to compute prediction expressed in seconds.
950 * @param jacobianQ jacobian wrt input quaternion. Must be 4x4.
951 * @param jacobianDQ jacobian wrt dq quaternion. Must be 4x4.
952 * @param jacobianW jacobian wrt angular speed. Must be 4x3.
953 * @return a new updated quaternion.
954 * @throws IllegalArgumentException if any of provided jacobians does not
955 * have proper size.
956 */
957 public static Quaternion predictWithRotationAdjustment(
958 final Quaternion q, final Quaternion dq, final double wx, final double wy, final double wz, final double dt,
959 final Matrix jacobianQ, final Matrix jacobianDQ, final Matrix jacobianW) {
960 final var result = new Quaternion();
961 predictWithRotationAdjustment(q, dq, wx, wy, wz, dt, result, jacobianQ, jacobianDQ, jacobianW);
962 return result;
963 }
964
965 /**
966 * Predicts the updated quaternion after a rotation in body frame expressed
967 * by provided rotation dq and by provided rate of rotation along axes x,y,z
968 * (roll, pitch, yaw).
969 *
970 * @param q quaternion to be updated.
971 * @param dq adjustment of rotation to be combined with input quaternion.
972 * @param w angular speed (x,y,z axes). Expressed in rad/s. Must have length
973 * 3.
974 * @param dt time interval to compute prediction expressed in seconds.
975 * @param jacobianQ jacobian wrt input quaternion. Must be 4x4.
976 * @param jacobianDQ jacobian wrt dq quaternion. Must be 4x4.
977 * @param jacobianW jacobian wrt angular speed. Must be 4x3.
978 * @return a new updated quaternion.
979 * @throws IllegalArgumentException if any of provided jacobians does not
980 * have proper size.
981 */
982 public static Quaternion predictWithRotationAdjustment(
983 final Quaternion q, final Quaternion dq, final double[] w, final double dt, final Matrix jacobianQ,
984 final Matrix jacobianDQ, final Matrix jacobianW) {
985 final var result = new Quaternion();
986 predictWithRotationAdjustment(q, dq, w, dt, result, jacobianQ, jacobianDQ, jacobianW);
987 return result;
988 }
989
990 /**
991 * Predicts the updated quaternion after a rotation in body frame expressed
992 * by provided rotation dq and by provided rate of rotation along axes x,y,z
993 * (roll, pitch, yaw).
994 *
995 * @param q quaternion to be updated.
996 * @param dq adjustment of rotation to be combined with input quaternion.
997 * @param wx angular speed in x-axis (roll axis). Expressed in rad/s.
998 * @param wy angular speed in y-axis (pitch axis). Expressed in rad/s.
999 * @param wz angular speed in z-axis (yaw axis). Expressed in rad/s.
1000 * @param dt time interval to compute prediction expressed in seconds.
1001 * @return a new updated quaternion.
1002 */
1003 public static Quaternion predictWithRotationAdjustment(
1004 final Quaternion q, final Quaternion dq, final double wx, final double wy, final double wz,
1005 final double dt) {
1006 final var result = new Quaternion();
1007 predictWithRotationAdjustment(q, dq, wx, wy, wz, dt, result);
1008 return result;
1009 }
1010
1011 /**
1012 * Predicts the updated quaternion after a rotation in body frame expressed
1013 * by provided rotation dq and by provided rate of rotation along axes x,y,z
1014 * (roll, pitch, yaw).
1015 *
1016 * @param q quaternion to be updated.
1017 * @param dq adjustment of rotation to be combined with input quaternion.
1018 * @param w angular speed (x, y, z axes). Expressed in rad/s. Must have
1019 * length 3.
1020 * @param dt time interval to compute prediction expressed in seconds.
1021 * @return a new updated quaternion.
1022 * @throws IllegalArgumentException if w does not have length 3.
1023 */
1024 public static Quaternion predictWithRotationAdjustment(
1025 final Quaternion q, final Quaternion dq, final double[] w, final double dt) {
1026 final var result = new Quaternion();
1027 predictWithRotationAdjustment(q, dq, w, dt, result);
1028 return result;
1029 }
1030 }