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.Matrix; 19 import com.irurueta.algebra.WrongSizeException; 20 import com.irurueta.geometry.InhomogeneousPoint3D; 21 import com.irurueta.geometry.Point3D; 22 import com.irurueta.geometry.Quaternion; 23 24 /** 25 * Utility class to predict device state (position, orientation, linear 26 * velocity, linear acceleration and angular velocity). 27 */ 28 @SuppressWarnings("DuplicatedCode") 29 public class StatePredictor { 30 31 /** 32 * Number of components on angular speed. 33 */ 34 public static final int ANGULAR_SPEED_COMPONENTS = 3; 35 36 /** 37 * Number of components of speed. 38 */ 39 public static final int SPEED_COMPONENTS = 3; 40 41 /** 42 * Number of components of acceleration. 43 */ 44 public static final int ACCELERATION_COMPONENTS = 3; 45 46 /** 47 * Number of components of constant acceleration model state. 48 */ 49 public static final int STATE_COMPONENTS = 16; 50 51 /** 52 * Number of components of constant acceleration model control signal. 53 */ 54 public static final int CONTROL_COMPONENTS = 9; 55 56 /** 57 * Number of components of constant acceleration model state with position 58 * adjustment. 59 */ 60 public static final int STATE_WITH_POSITION_ADJUSTMENT_COMPONENTS = 16; 61 62 /** 63 * Number of components of constant acceleration model with position 64 * adjustment control signal. 65 */ 66 public static final int CONTROL_WITH_POSITION_ADJUSTMENT_COMPONENTS = 12; 67 68 /** 69 * Number of components of constant acceleration model state with rotation 70 * adjustment. 71 */ 72 public static final int STATE_WITH_ROTATION_ADJUSTMENT_COMPONENTS = 16; 73 74 /** 75 * Number of components of constant acceleration model with rotation 76 * adjustment control signal. 77 */ 78 public static final int CONTROL_WITH_ROTATION_ADJUSTMENT_COMPONENTS = 13; 79 80 /** 81 * Number of components of constant acceleration model with position and 82 * rotation adjustment. 83 */ 84 public static final int STATE_WITH_POSITION_AND_ROTATION_ADJUSTMENT_COMPONENTS = 16; 85 86 /** 87 * Number of components of constant acceleration model with position and 88 * rotation adjustment control signal. 89 */ 90 public static final int CONTROL_WITH_POSITION_AND_ROTATION_ADJUSTMENT_COMPONENTS = 16; 91 92 /** 93 * Constructor. 94 */ 95 private StatePredictor() { 96 } 97 98 /** 99 * Updates the system model (position, orientation, linear velocity, 100 * linear acceleration and angular velocity) assuming a constant 101 * acceleration model when no acceleration or velocity control signal is 102 * present. 103 * 104 * @param x initial system state containing: position-x, position-y, 105 * position-z, quaternion-a, quaternion-b, quaternion-c, quaternion-d, 106 * linear-velocity-x, linear-velocity-y, linear-velocity-z, 107 * linear-acceleration-x, linear-acceleration-y, linear-acceleration-z, 108 * angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have 109 * length 16. 110 * @param u perturbations or control signals: linear-velocity-change-x, 111 * linear-velocity-change-y, linear-velocity-change-z, 112 * linear-acceleration-change-x, linear-acceleration-change-y, 113 * linear-acceleration-change-z, angular-velocity-change-x, 114 * angular-velocity-change-y, angular-velocity-change-z. Must have length 9. 115 * @param dt time interval to compute prediction expressed in seconds. 116 * @param result instance where updated system model will be stored. Must 117 * have length 16. 118 * @param jacobianX jacobian wrt system state. Must be 16x16. 119 * @param jacobianU jacobian wrt control. must be 16x9. 120 * @throws IllegalArgumentException if system state array, control array, 121 * result or jacobians do not have proper size. 122 */ 123 public static void predict( 124 final double[] x, final double[] u, final double dt, final double[] result, final Matrix jacobianX, 125 final Matrix jacobianU) { 126 if (x.length != STATE_COMPONENTS) { 127 // x must have length 16 128 throw new IllegalArgumentException(); 129 } 130 if (u.length != CONTROL_COMPONENTS) { 131 // u must have length 9 132 throw new IllegalArgumentException(); 133 } 134 if (result.length != STATE_COMPONENTS) { 135 // result must have length 16 136 throw new IllegalArgumentException(); 137 } 138 if (jacobianX != null && (jacobianX.getRows() != STATE_COMPONENTS 139 || jacobianX.getColumns() != STATE_COMPONENTS)) { 140 // jacobian wrt x must be 16x16 141 throw new IllegalArgumentException(); 142 } 143 if (jacobianU != null && (jacobianU.getRows() != STATE_COMPONENTS 144 || jacobianU.getColumns() != CONTROL_COMPONENTS)) { 145 // jacobian wrt u must be 16x9 146 throw new IllegalArgumentException(); 147 } 148 149 // position 150 final var r = new InhomogeneousPoint3D(x[0], x[1], x[2]); 151 152 // orientation 153 var q = new Quaternion(x[3], x[4], x[5], x[6]); 154 155 // linear velocity 156 var vx = x[7]; 157 var vy = x[8]; 158 var vz = x[9]; 159 160 // linear acceleration 161 var ax = x[10]; 162 var ay = x[11]; 163 var az = x[12]; 164 165 // angular velocity 166 var wx = x[13]; 167 var wy = x[14]; 168 var wz = x[15]; 169 170 // linear velocity change (control) 171 final var uvx = u[0]; 172 final var uvy = u[1]; 173 final var uvz = u[2]; 174 175 // linear acceleration change (control) 176 final var uax = u[3]; 177 final var uay = u[4]; 178 final var uaz = u[5]; 179 180 // angular velocity change (control) 181 final var uwx = u[6]; 182 final var uwy = u[7]; 183 final var uwz = u[8]; 184 185 try { 186 // update velocity 187 final var vv = new Matrix(SPEED_COMPONENTS, SPEED_COMPONENTS); 188 final var va = new Matrix(SPEED_COMPONENTS, ACCELERATION_COMPONENTS); 189 final var v = VelocityPredictor.predict(vx, vy, vz, ax, ay, az, dt, vv, va); 190 191 // update position 192 Matrix rr = null; 193 Matrix rv = null; 194 Matrix ra = null; 195 if (jacobianX != null) { 196 rr = new Matrix(Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH, 197 Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH); 198 rv = new Matrix(Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH, SPEED_COMPONENTS); 199 ra = new Matrix(Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH, ACCELERATION_COMPONENTS); 200 } 201 PositionPredictor.predict(r, vx, vy, vz, ax, ay, az, dt, r, rr, rv, ra); 202 203 // update orientation 204 Matrix qq = null; 205 Matrix qw = null; 206 if (jacobianX != null) { 207 qq = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS); 208 qw = new Matrix(Quaternion.N_PARAMS, ANGULAR_SPEED_COMPONENTS); 209 } 210 q = QuaternionPredictor.predict(q, wx, wy, wz, dt, true, qq, qw); 211 212 // set updated linear velocity 213 vx = v[0]; 214 vy = v[1]; 215 vz = v[2]; 216 217 // apply control signals 218 vx += uvx; 219 vy += uvy; 220 vz += uvz; 221 222 ax += uax; 223 ay += uay; 224 az += uaz; 225 226 wx += uwx; 227 wy += uwy; 228 wz += uwz; 229 230 // set new state 231 result[0] = r.getInhomX(); 232 result[1] = r.getInhomY(); 233 result[2] = r.getInhomZ(); 234 235 result[3] = q.getA(); 236 result[4] = q.getB(); 237 result[5] = q.getC(); 238 result[6] = q.getD(); 239 240 result[7] = vx; 241 result[8] = vy; 242 result[9] = vz; 243 244 result[10] = ax; 245 result[11] = ay; 246 result[12] = az; 247 248 result[13] = wx; 249 result[14] = wy; 250 result[15] = wz; 251 252 // jacobians 253 if (jacobianX != null) { 254 // [Rr 0 Rv Ra 0 ] 255 // [0 Qq 0 0 Qw ] 256 // [0 0 Vv Va 0 ] 257 // [0 0 0 eye 0 ] 258 // [0 0 0 0 eye] 259 jacobianX.initialize(0.0); 260 jacobianX.setSubmatrix(0, 0, 2, 2, rr); 261 262 jacobianX.setSubmatrix(3, 3, 6, 6, qq); 263 264 jacobianX.setSubmatrix(0, 7, 2, 9, rv); 265 266 jacobianX.setSubmatrix(7, 7, 9, 9, vv); 267 268 jacobianX.setSubmatrix(0, 10, 2, 12, ra); 269 270 jacobianX.setSubmatrix(7, 10, 9, 12, va); 271 272 jacobianX.setSubmatrix(3, 13, 6, 15, qw); 273 274 for (var i = 10; i < STATE_COMPONENTS; i++) { 275 jacobianX.setElementAt(i, i, 1.0); 276 } 277 } 278 279 if (jacobianU != null) { 280 jacobianU.initialize(0.0); 281 282 for (int i = 7, j = 0; i < STATE_COMPONENTS; i++, j++) { 283 jacobianU.setElementAt(i, j, 1.0); 284 } 285 } 286 } catch (final WrongSizeException ignore) { 287 // never thrown 288 } 289 } 290 291 /** 292 * Updates the system model (position, orientation, linear velocity, 293 * linear acceleration and angular velocity) assuming a constant 294 * acceleration model when no acceleration or velocity control signal is 295 * present. 296 * 297 * @param x initial system state containing: position-x, position-y, 298 * position-z, quaternion-a, quaternion-b, quaternion-c, quaternion-d, 299 * linear-velocity-x, linear-velocity-y, linear-velocity-z, 300 * linear-acceleration-x, linear-acceleration-y, linear-acceleration-z, 301 * angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have 302 * length 16. 303 * @param u perturbations or control signals: linear-velocity-change-x, 304 * linear-velocity-change-y, linear-velocity-change-z, 305 * linear-acceleration-change-x, linear-acceleration-change-y, 306 * linear-acceleration-change-z, angular-velocity-change-x, 307 * angular-velocity-change-y, angular-velocity-change-z. Must have length 9. 308 * @param dt time interval to compute prediction expressed in seconds. 309 * @param result instance where updated system model will be stored. Must 310 * have length 16. 311 * @throws IllegalArgumentException if system state array, control array 312 * or result do not have proper size. 313 */ 314 public static void predict(final double[] x, final double[] u, final double dt, final double[] result) { 315 predict(x, u, dt, result, null, null); 316 } 317 318 /** 319 * Updates the system model (position, orientation, linear velocity, 320 * linear acceleration and angular velocity) assuming a constant 321 * acceleration model when no acceleration or velocity control signal is 322 * present. 323 * 324 * @param x initial system state containing: position-x, position-y, 325 * position-z, quaternion-a, quaternion-b, quaternion-c, quaternion-d, 326 * linear-velocity-x, linear-velocity-y, linear-velocity-z, 327 * linear-acceleration-x, linear-acceleration-y, linear-acceleration-z, 328 * angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have 329 * length 16. 330 * @param u perturbations or control signals: linear-velocity-change-x, 331 * linear-velocity-change-y, linear-velocity-change-z, 332 * linear-acceleration-change-x, linear-acceleration-change-y, 333 * linear-acceleration-change-z, angular-velocity-change-x, 334 * angular-velocity-change-y, angular-velocity-change-z. Must have length 9. 335 * @param dt time interval to compute prediction expressed in seconds. 336 * @param jacobianX jacobian wrt system state. Must be 16x16. 337 * @param jacobianU jacobian wrt control. must be 16x9. 338 * @return a new instance containing the updated system state. 339 * @throws IllegalArgumentException if system state array, control array or 340 * jacobians do not have proper size. 341 */ 342 public static double[] predict( 343 final double[] x, final double[] u, final double dt, final Matrix jacobianX, final Matrix jacobianU) { 344 final var result = new double[STATE_COMPONENTS]; 345 predict(x, u, dt, result, jacobianX, jacobianU); 346 return result; 347 } 348 349 /** 350 * Updates the system model (position, orientation, linear velocity, 351 * linear acceleration and angular velocity) assuming a constant 352 * acceleration model when no acceleration or velocity control signal is 353 * present. 354 * 355 * @param x initial system state containing: position-x, position-y, 356 * position-z, quaternion-a, quaternion-b, quaternion-c, quaternion-d, 357 * linear-velocity-x, linear-velocity-y, linear-velocity-z, 358 * linear-acceleration-x, linear-acceleration-y, linear-acceleration-z, 359 * angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have 360 * length 16. 361 * @param u perturbations or control signals: linear-velocity-change-x, 362 * linear-velocity-change-y, linear-velocity-change-z, 363 * linear-acceleration-change-x, linear-acceleration-change-y, 364 * linear-acceleration-change-z, angular-velocity-change-x, 365 * angular-velocity-change-y, angular-velocity-change-z. Must have length 9. 366 * @param dt time interval to compute prediction expressed in seconds. 367 * @return a new instance containing the updated system state. 368 * @throws IllegalArgumentException if system state array or control array 369 * do not have proper size. 370 */ 371 public static double[] predict(final double[] x, final double[] u, final double dt) { 372 final var result = new double[STATE_COMPONENTS]; 373 predict(x, u, dt, result); 374 return result; 375 } 376 377 /** 378 * Updates the system model (position, orientation, linear velocity, 379 * linear acceleration and angular velocity) assuming a constant 380 * acceleration model when no acceleration or velocity control signal is 381 * present. 382 * 383 * @param x initial system state containing: position-x, position-y, 384 * position-z, quaternion-a, quaternion-b, quaternion-c, quaternion-d, 385 * linear-velocity-x, linear-velocity-y, linear-velocity-z, 386 * linear-acceleration-x, linear-acceleration-y, linear-acceleration-z, 387 * angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have 388 * length 16. 389 * @param u perturbations or control signals: position-change-x, 390 * position-change-y, position-change-z, linear-velocity-change-x, 391 * linear-velocity-change-y, linear-velocity-change-z, 392 * linear-acceleration-change-x, linear-acceleration-change-y, 393 * linear-acceleration-change-z, angular-velocity-change-x, 394 * angular-velocity-change-y, angular-velocity-change-z. Must have length 395 * 12. 396 * @param dt time interval to compute prediction expressed in seconds. 397 * @param result instance where updated system model will be stored. Must 398 * have length 16. 399 * @param jacobianX jacobian wrt system state. Must be 16x16. 400 * @param jacobianU jacobian wrt control. must be 16x12. 401 * @throws IllegalArgumentException if system state array, control array, 402 * result array or jacobians do not have proper size. 403 */ 404 public static void predictWithPositionAdjustment( 405 final double[] x, final double[] u, final double dt, final double[] result, final Matrix jacobianX, 406 final Matrix jacobianU) { 407 if (x.length != STATE_WITH_POSITION_ADJUSTMENT_COMPONENTS) { 408 // x must have length 16 409 throw new IllegalArgumentException(); 410 } 411 if (u.length != CONTROL_WITH_POSITION_ADJUSTMENT_COMPONENTS) { 412 // u must have length 12 413 throw new IllegalArgumentException(); 414 } 415 if (result.length != STATE_WITH_POSITION_ADJUSTMENT_COMPONENTS) { 416 // result must have length 16 417 throw new IllegalArgumentException(); 418 } 419 if (jacobianX != null && (jacobianX.getRows() != STATE_WITH_POSITION_ADJUSTMENT_COMPONENTS 420 || jacobianX.getColumns() != STATE_WITH_POSITION_ADJUSTMENT_COMPONENTS)) { 421 // jacobian wrt x must be 16x16 422 throw new IllegalArgumentException(); 423 } 424 if (jacobianU != null && (jacobianU.getRows() != STATE_WITH_POSITION_ADJUSTMENT_COMPONENTS 425 || jacobianU.getColumns() != CONTROL_WITH_POSITION_ADJUSTMENT_COMPONENTS)) { 426 // jacobian wrt u must be 16x12 427 throw new IllegalArgumentException(); 428 } 429 430 // position 431 final var r = new InhomogeneousPoint3D(x[0], x[1], x[2]); 432 433 // orientation 434 var q = new Quaternion(x[3], x[4], x[5], x[6]); 435 436 // linear velocity 437 var vx = x[7]; 438 var vy = x[8]; 439 var vz = x[9]; 440 441 // linear acceleration 442 var ax = x[10]; 443 var ay = x[11]; 444 var az = x[12]; 445 446 // angular velocity 447 var wx = x[13]; 448 var wy = x[14]; 449 var wz = x[15]; 450 451 // position change (control) 452 final var drx = u[0]; 453 final var dry = u[1]; 454 final var drz = u[2]; 455 456 // linear velocity change (control) 457 final var uvx = u[3]; 458 final var uvy = u[4]; 459 final var uvz = u[5]; 460 461 // linear acceleration change (control) 462 final var uax = u[6]; 463 final var uay = u[7]; 464 final var uaz = u[8]; 465 466 // angular velocity change (control) 467 final var uwx = u[9]; 468 final var uwy = u[10]; 469 final var uwz = u[11]; 470 471 try { 472 // update velocity 473 final var vv = new Matrix(SPEED_COMPONENTS, SPEED_COMPONENTS); 474 final var va = new Matrix(SPEED_COMPONENTS, ACCELERATION_COMPONENTS); 475 final var v = VelocityPredictor.predict(vx, vy, vz, ax, ay, az, dt, vv, va); 476 477 // update position 478 Matrix rr = null; 479 Matrix rv = null; 480 Matrix ra = null; 481 if (jacobianX != null) { 482 rr = new Matrix(Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH, 483 Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH); 484 rv = new Matrix(Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH, SPEED_COMPONENTS); 485 ra = new Matrix(Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH, ACCELERATION_COMPONENTS); 486 } 487 PositionPredictor.predictWithPositionAdjustment(r, drx, dry, drz, vx, vy, vz, ax, ay, az, dt, r, rr, 488 null, rv, ra); 489 490 // update orientation 491 Matrix qq = null; 492 Matrix qw = null; 493 if (jacobianX != null) { 494 qq = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS); 495 qw = new Matrix(Quaternion.N_PARAMS, ANGULAR_SPEED_COMPONENTS); 496 } 497 q = QuaternionPredictor.predict(q, wx, wy, wz, dt, true, qq, qw); 498 499 // set updated linear velocity 500 vx = v[0]; 501 vy = v[1]; 502 vz = v[2]; 503 504 // apply control signals 505 vx += uvx; 506 vy += uvy; 507 vz += uvz; 508 509 ax += uax; 510 ay += uay; 511 az += uaz; 512 513 wx += uwx; 514 wy += uwy; 515 wz += uwz; 516 517 // set new state 518 result[0] = r.getInhomX(); 519 result[1] = r.getInhomY(); 520 result[2] = r.getInhomZ(); 521 522 result[3] = q.getA(); 523 result[4] = q.getB(); 524 result[5] = q.getC(); 525 result[6] = q.getD(); 526 527 result[7] = vx; 528 result[8] = vy; 529 result[9] = vz; 530 531 result[10] = ax; 532 result[11] = ay; 533 result[12] = az; 534 535 result[13] = wx; 536 result[14] = wy; 537 result[15] = wz; 538 539 // jacobians 540 if (jacobianX != null) { 541 // [Rr 0 Rv Ra 0 ] 542 // [0 Qq 0 0 Qw ] 543 // [0 0 Vv Va 0 ] 544 // [0 0 0 eye 0 ] 545 // [0 0 0 0 eye] 546 jacobianX.initialize(0.0); 547 jacobianX.setSubmatrix(0, 0, 2, 2, rr); 548 549 jacobianX.setSubmatrix(3, 3, 6, 6, qq); 550 551 jacobianX.setSubmatrix(0, 7, 2, 9, rv); 552 553 jacobianX.setSubmatrix(7, 7, 9, 9, vv); 554 555 jacobianX.setSubmatrix(0, 10, 2, 12, ra); 556 557 jacobianX.setSubmatrix(7, 10, 9, 12, va); 558 559 jacobianX.setSubmatrix(3, 13, 6, 15, qw); 560 561 for (var i = 10; i < STATE_WITH_POSITION_ADJUSTMENT_COMPONENTS; i++) { 562 jacobianX.setElementAt(i, i, 1.0); 563 } 564 } 565 566 if (jacobianU != null) { 567 jacobianU.initialize(0.0); 568 569 // variation of position 570 for (var i = 0; i < Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH; i++) { 571 jacobianU.setElementAt(i, i, 1.0); 572 } 573 // variation of linear speed and acceleration, and angular speed 574 for (int i = 7, j = Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH; 575 i < STATE_WITH_POSITION_ADJUSTMENT_COMPONENTS; i++, j++) { 576 jacobianU.setElementAt(i, j, 1.0); 577 } 578 } 579 } catch (final WrongSizeException ignore) { 580 // never thrown 581 } 582 } 583 584 /** 585 * Updates the system model (position, orientation, linear velocity, 586 * linear acceleration and angular velocity) assuming a constant 587 * acceleration model when no acceleration or velocity control signal is 588 * present. 589 * 590 * @param x initial system state containing: position-x, position-y, 591 * position-z, quaternion-a, quaternion-b, quaternion-c, quaternion-d, 592 * linear-velocity-x, linear-velocity-y, linear-velocity-z, 593 * linear-acceleration-x, linear-acceleration-y, linear-acceleration-z, 594 * angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have 595 * length 16. 596 * @param u perturbations or control signals: position-change-x, 597 * position-change-y, position-change-z, linear-velocity-change-x, 598 * linear-velocity-change-y, linear-velocity-change-z, 599 * linear-acceleration-change-x, linear-acceleration-change-y, 600 * linear-acceleration-change-z, angular-velocity-change-x, 601 * angular-velocity-change-y, angular-velocity-change-z. Must have length 602 * 12. 603 * @param dt time interval to compute prediction expressed in seconds. 604 * @param result instance where updated system model will be stored. Must 605 * have length 16. 606 * @throws IllegalArgumentException if system state array, control array or 607 * result array do not have proper size. 608 */ 609 public static void predictWithPositionAdjustment( 610 final double[] x, final double[] u, final double dt, final double[] result) { 611 predictWithPositionAdjustment(x, u, dt, result, null, null); 612 } 613 614 /** 615 * Updates the system model (position, orientation, linear velocity, 616 * linear acceleration and angular velocity) assuming a constant 617 * acceleration model when no acceleration or velocity control signal is 618 * present. 619 * 620 * @param x initial system state containing: position-x, position-y, 621 * position-z, quaternion-a, quaternion-b, quaternion-c, quaternion-d, 622 * linear-velocity-x, linear-velocity-y, linear-velocity-z, 623 * linear-acceleration-x, linear-acceleration-y, linear-acceleration-z, 624 * angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have 625 * length 16. 626 * @param u perturbations or control signals: position-change-x, 627 * position-change-y, position-change-z, linear-velocity-change-x, 628 * linear-velocity-change-y, linear-velocity-change-z, 629 * linear-acceleration-change-x, linear-acceleration-change-y, 630 * linear-acceleration-change-z, angular-velocity-change-x, 631 * angular-velocity-change-y, angular-velocity-change-z. Must have length 632 * 12. 633 * @param dt time interval to compute prediction expressed in seconds. 634 * @param jacobianX jacobian wrt system state. Must be 16x16. 635 * @param jacobianU jacobian wrt control. must be 16x12. 636 * @return a new array containing updated system model. 637 * @throws IllegalArgumentException if system state array, control array 638 * or jacobians do not have proper size. 639 */ 640 public static double[] predictWithPositionAdjustment( 641 final double[] x, final double[] u, final double dt, final Matrix jacobianX, final Matrix jacobianU) { 642 final var result = new double[STATE_WITH_POSITION_ADJUSTMENT_COMPONENTS]; 643 predictWithPositionAdjustment(x, u, dt, result, jacobianX, jacobianU); 644 return result; 645 } 646 647 /** 648 * Updates the system model (position, orientation, linear velocity, 649 * linear acceleration and angular velocity) assuming a constant 650 * acceleration model when no acceleration or velocity control signal is 651 * present. 652 * 653 * @param x initial system state containing: position-x, position-y, 654 * position-z, quaternion-a, quaternion-b, quaternion-c, quaternion-d, 655 * linear-velocity-x, linear-velocity-y, linear-velocity-z, 656 * linear-acceleration-x, linear-acceleration-y, linear-acceleration-z, 657 * angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have 658 * length 16. 659 * @param u perturbations or control signals: position-change-x, 660 * position-change-y, position-change-z, linear-velocity-change-x, 661 * linear-velocity-change-y, linear-velocity-change-z, 662 * linear-acceleration-change-x, linear-acceleration-change-y, 663 * linear-acceleration-change-z, angular-velocity-change-x, 664 * angular-velocity-change-y, angular-velocity-change-z. Must have length 665 * 12. 666 * @param dt time interval to compute prediction expressed in seconds. 667 * @return a new array containing updated system model. 668 * @throws IllegalArgumentException if system state array or control array 669 * do not have proper size. 670 */ 671 public static double[] predictWithPositionAdjustment(final double[] x, final double[] u, final double dt) { 672 final var result = new double[STATE_WITH_POSITION_ADJUSTMENT_COMPONENTS]; 673 predictWithPositionAdjustment(x, u, dt, result); 674 return result; 675 } 676 677 /** 678 * Updates the system model (position, orientation, linear velocity, 679 * linear acceleration and angular velocity) assuming a constant 680 * acceleration model when no acceleration or velocity control signal is 681 * present. 682 * 683 * @param x initial system state containing: position-x, position-y, 684 * position-z, quaternion-a, quaternion-b, quaternion-c, quaternion-d, 685 * linear-velocity-x, linear-velocity-y, linear-velocity-z, 686 * linear-acceleration-x, linear-acceleration-y, linear-acceleration-z, 687 * angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have 688 * length 16. 689 * @param u perturbations or control signals: quaternion-change-a, 690 * quaternion-change-b, quaternion-change-c, quaternion-change-d, 691 * linear-velocity-change-x, linear-velocity-change-y, 692 * linear-velocity-change-z, linear-acceleration-change-x, 693 * linear-acceleration-change-y, linear-acceleration-change-z, 694 * angular-velocity-change-x, angular-velocity-change-y, 695 * angular-velocity-change-z. Must have length 13. 696 * @param dt time interval to compute prediction expressed in seconds. 697 * @param result instance where updated system model will be stored. Must 698 * have length 16. 699 * @param jacobianX jacobian wrt system state. Must be 16x16. 700 * @param jacobianU jacobian wrt control. must be 16x13. 701 * @throws IllegalArgumentException if system state array, control array, 702 * result array or jacobians do not have proper size. 703 */ 704 public static void predictWithRotationAdjustment( 705 final double[] x, final double[] u, final double dt, final double[] result, final Matrix jacobianX, 706 final Matrix jacobianU) { 707 if (x.length != STATE_WITH_ROTATION_ADJUSTMENT_COMPONENTS) { 708 throw new IllegalArgumentException("x must have length 16"); 709 } 710 if (u.length != CONTROL_WITH_ROTATION_ADJUSTMENT_COMPONENTS) { 711 throw new IllegalArgumentException("u must have length 13"); 712 } 713 if (result.length != STATE_WITH_ROTATION_ADJUSTMENT_COMPONENTS) { 714 throw new IllegalArgumentException("result must have length 16"); 715 } 716 if (jacobianX != null && (jacobianX.getRows() != STATE_WITH_ROTATION_ADJUSTMENT_COMPONENTS 717 || jacobianX.getColumns() != STATE_WITH_ROTATION_ADJUSTMENT_COMPONENTS)) { 718 throw new IllegalArgumentException("jacobian wrt x must be 16x16"); 719 } 720 if (jacobianU != null && (jacobianU.getRows() != STATE_WITH_ROTATION_ADJUSTMENT_COMPONENTS 721 || jacobianU.getColumns() != CONTROL_WITH_ROTATION_ADJUSTMENT_COMPONENTS)) { 722 throw new IllegalArgumentException("jacobian wrt u must be 16x13"); 723 } 724 725 // position 726 final var r = new InhomogeneousPoint3D(x[0], x[1], x[2]); 727 728 // orientation 729 var q = new Quaternion(x[3], x[4], x[5], x[6]); 730 731 // linear velocity 732 var vx = x[7]; 733 var vy = x[8]; 734 var vz = x[9]; 735 736 // linear acceleration 737 var ax = x[10]; 738 var ay = x[11]; 739 var az = x[12]; 740 741 // angular velocity 742 var wx = x[13]; 743 var wy = x[14]; 744 var wz = x[15]; 745 746 // rotation change (control) 747 final var dq = new Quaternion(u[0], u[1], u[2], u[3]); 748 749 // linear velocity change (control) 750 final var uvx = u[4]; 751 final var uvy = u[5]; 752 final var uvz = u[6]; 753 754 // linear acceleration change (control) 755 final var uax = u[7]; 756 final var uay = u[8]; 757 final var uaz = u[9]; 758 759 // angular velocity change (control) 760 final var uwx = u[10]; 761 final var uwy = u[11]; 762 final var uwz = u[12]; 763 764 try { 765 // update velocity 766 final var vv = new Matrix(SPEED_COMPONENTS, SPEED_COMPONENTS); 767 final var va = new Matrix(SPEED_COMPONENTS, ACCELERATION_COMPONENTS); 768 final var v = VelocityPredictor.predict(vx, vy, vz, ax, ay, az, dt, vv, va); 769 770 // update position 771 Matrix rr = null; 772 Matrix rv = null; 773 Matrix ra = null; 774 if (jacobianX != null) { 775 rr = new Matrix(Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH, 776 Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH); 777 rv = new Matrix(Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH, SPEED_COMPONENTS); 778 ra = new Matrix(Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH, ACCELERATION_COMPONENTS); 779 } 780 PositionPredictor.predict(r, vx, vy, vz, ax, ay, az, dt, r, rr, rv, ra); 781 782 // update orientation 783 Matrix qq = null; 784 Matrix qdq = null; 785 Matrix qw = null; 786 if (jacobianX != null) { 787 qq = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS); 788 qw = new Matrix(Quaternion.N_PARAMS, ANGULAR_SPEED_COMPONENTS); 789 } 790 if (jacobianU != null) { 791 qdq = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS); 792 } 793 q = QuaternionPredictor.predictWithRotationAdjustment(q, dq, wx, wy, wz, dt, qq, qdq, qw); 794 795 796 // set updated linear velocity 797 vx = v[0]; 798 vy = v[1]; 799 vz = v[2]; 800 801 // apply control signals 802 vx += uvx; 803 vy += uvy; 804 vz += uvz; 805 806 ax += uax; 807 ay += uay; 808 az += uaz; 809 810 wx += uwx; 811 wy += uwy; 812 wz += uwz; 813 814 // set new state 815 result[0] = r.getInhomX(); 816 result[1] = r.getInhomY(); 817 result[2] = r.getInhomZ(); 818 819 result[3] = q.getA(); 820 result[4] = q.getB(); 821 result[5] = q.getC(); 822 result[6] = q.getD(); 823 824 result[7] = vx; 825 result[8] = vy; 826 result[9] = vz; 827 828 result[10] = ax; 829 result[11] = ay; 830 result[12] = az; 831 832 result[13] = wx; 833 result[14] = wy; 834 result[15] = wz; 835 836 // jacobians 837 if (jacobianX != null) { 838 // [Rr 0 Rv Ra 0 ] 839 // [0 Qq 0 0 Qw ] 840 // [0 0 Vv Va 0 ] 841 // [0 0 0 eye 0 ] 842 // [0 0 0 0 eye] 843 jacobianX.initialize(0.0); 844 jacobianX.setSubmatrix(0, 0, 2, 2, rr); 845 846 jacobianX.setSubmatrix(3, 3, 6, 6, qq); 847 848 jacobianX.setSubmatrix(0, 7, 2, 9, rv); 849 850 jacobianX.setSubmatrix(7, 7, 9, 9, vv); 851 852 jacobianX.setSubmatrix(0, 10, 2, 12, ra); 853 854 jacobianX.setSubmatrix(7, 10, 9, 12, va); 855 856 jacobianX.setSubmatrix(3, 13, 6, 15, qw); 857 858 for (var i = 10; i < STATE_WITH_ROTATION_ADJUSTMENT_COMPONENTS; i++) { 859 jacobianX.setElementAt(i, i, 1.0); 860 } 861 } 862 863 if (jacobianU != null) { 864 jacobianU.initialize(0.0); 865 866 // variation of rotation 867 jacobianU.setSubmatrix(3, 0, 6, 3, qdq); 868 869 // variation of linear speed and acceleration, and angular speed 870 for (int i = 7, j = Quaternion.N_PARAMS; i < STATE_WITH_ROTATION_ADJUSTMENT_COMPONENTS; i++, j++) { 871 jacobianU.setElementAt(i, j, 1.0); 872 } 873 } 874 } catch (final WrongSizeException ignore) { 875 // never thrown 876 } 877 } 878 879 /** 880 * Updates the system model (position, orientation, linear velocity, 881 * linear acceleration and angular velocity) assuming a constant 882 * acceleration model when no acceleration or velocity control signal is 883 * present. 884 * 885 * @param x initial system state containing: position-x, position-y, 886 * position-z, quaternion-a, quaternion-b, quaternion-c, quaternion-d, 887 * linear-velocity-x, linear-velocity-y, linear-velocity-z, 888 * linear-acceleration-x, linear-acceleration-y, linear-acceleration-z, 889 * angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have 890 * length 16. 891 * @param u perturbations or control signals: quaternion-change-a, 892 * quaternion-change-b, quaternion-change-c, quaternion-change-d, 893 * linear-velocity-change-x, linear-velocity-change-y, 894 * linear-velocity-change-z, linear-acceleration-change-x, 895 * linear-acceleration-change-y, linear-acceleration-change-z, 896 * angular-velocity-change-x, angular-velocity-change-y, 897 * angular-velocity-change-z. Must have length 13. 898 * @param dt time interval to compute prediction expressed in seconds. 899 * @param result instance where updated system model will be stored. Must 900 * have length 16. 901 * @throws IllegalArgumentException if system state array, control array or 902 * result array do not have proper size. 903 */ 904 public static void predictWithRotationAdjustment( 905 final double[] x, final double[] u, final double dt, final double[] result) { 906 predictWithRotationAdjustment(x, u, dt, result, null, null); 907 } 908 909 /** 910 * Updates the system model (position, orientation, linear velocity, 911 * linear acceleration and angular velocity) assuming a constant 912 * acceleration model when no acceleration or velocity control signal is 913 * present. 914 * 915 * @param x initial system state containing: position-x, position-y, 916 * position-z, quaternion-a, quaternion-b, quaternion-c, quaternion-d, 917 * linear-velocity-x, linear-velocity-y, linear-velocity-z, 918 * linear-acceleration-x, linear-acceleration-y, linear-acceleration-z, 919 * angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have 920 * length 16. 921 * @param u perturbations or control signals: quaternion-change-a, 922 * quaternion-change-b, quaternion-change-c, quaternion-change-d, 923 * linear-velocity-change-x, linear-velocity-change-y, 924 * linear-velocity-change-z, linear-acceleration-change-x, 925 * linear-acceleration-change-y, linear-acceleration-change-z, 926 * angular-velocity-change-x, angular-velocity-change-y, 927 * angular-velocity-change-z. Must have length 13. 928 * @param dt time interval to compute prediction expressed in seconds. 929 * @param jacobianX jacobian wrt system state. Must be 16x16. 930 * @param jacobianU jacobian wrt control. must be 16x13. 931 * @return a new array containing updated system model. 932 * @throws IllegalArgumentException if system state array, control array or 933 * jacobians do not have proper size. 934 */ 935 public static double[] predictWithRotationAdjustment( 936 final double[] x, final double[] u, final double dt, final Matrix jacobianX, final Matrix jacobianU) { 937 final var result = new double[STATE_WITH_ROTATION_ADJUSTMENT_COMPONENTS]; 938 predictWithRotationAdjustment(x, u, dt, result, jacobianX, jacobianU); 939 return result; 940 } 941 942 /** 943 * Updates the system model (position, orientation, linear velocity, 944 * linear acceleration and angular velocity) assuming a constant 945 * acceleration model when no acceleration or velocity control signal is 946 * present. 947 * 948 * @param x initial system state containing: position-x, position-y, 949 * position-z, quaternion-a, quaternion-b, quaternion-c, quaternion-d, 950 * linear-velocity-x, linear-velocity-y, linear-velocity-z, 951 * linear-acceleration-x, linear-acceleration-y, linear-acceleration-z, 952 * angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have 953 * length 16. 954 * @param u perturbations or control signals: quaternion-change-a, 955 * quaternion-change-b, quaternion-change-c, quaternion-change-d, 956 * linear-velocity-change-x, linear-velocity-change-y, 957 * linear-velocity-change-z, linear-acceleration-change-x, 958 * linear-acceleration-change-y, linear-acceleration-change-z, 959 * angular-velocity-change-x, angular-velocity-change-y, 960 * angular-velocity-change-z. Must have length 13. 961 * @param dt time interval to compute prediction expressed in seconds. 962 * @return a new array containing updated system model. 963 * @throws IllegalArgumentException if system state array or control array 964 * do not have proper size. 965 */ 966 public static double[] predictWithRotationAdjustment(final double[] x, final double[] u, final double dt) { 967 final var result = new double[STATE_WITH_ROTATION_ADJUSTMENT_COMPONENTS]; 968 predictWithRotationAdjustment(x, u, dt, result); 969 return result; 970 } 971 972 /** 973 * Updates the system model (position, orientation, linear velocity, 974 * linear acceleration and angular velocity) assuming a constant 975 * acceleration model when no acceleration or velocity control signal is 976 * present. 977 * 978 * @param x initial system state containing: position-x, position-y, 979 * position-z, quaternion-a, quaternion-b, quaternion-c, quaternion-d, 980 * linear-velocity-x, linear-velocity-y, linear-velocity-z, 981 * linear-acceleration-x, linear-acceleration-y, linear-acceleration-z, 982 * angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have 983 * length 16. 984 * @param u perturbations or control signals: position-change-x, 985 * position-change-y, position-change-z, quaternion-change-a, 986 * quaternion-change-b, quaternion-change-c, quaternion-change-d, 987 * linear-velocity-change-x, linear-velocity-change-y, 988 * linear-velocity-change-z, linear-acceleration-change-x, 989 * linear-acceleration-change-y, linear-acceleration-change-z, 990 * angular-velocity-change-x, angular-velocity-change-y, 991 * angular-velocity-change-z. Must have length 16. 992 * @param dt time interval to compute prediction expressed in seconds. 993 * @param result instance where updated system model will be stored. Must 994 * have length 16. 995 * @param jacobianX jacobian wrt system state. Must be 16x16. 996 * @param jacobianU jacobian wrt control. must be 16x16. 997 * @throws IllegalArgumentException if system state array, control array, 998 * result array or jacobians do not have proper size. 999 */ 1000 public static void predictWithPositionAndRotationAdjustment( 1001 final double[] x, final double[] u, final double dt, final double[] result, final Matrix jacobianX, 1002 final Matrix jacobianU) { 1003 if (x.length != STATE_WITH_POSITION_AND_ROTATION_ADJUSTMENT_COMPONENTS) { 1004 throw new IllegalArgumentException("x must have length 16"); 1005 } 1006 if (u.length != CONTROL_WITH_POSITION_AND_ROTATION_ADJUSTMENT_COMPONENTS) { 1007 throw new IllegalArgumentException("u must have length 13"); 1008 } 1009 if (result.length != STATE_WITH_POSITION_AND_ROTATION_ADJUSTMENT_COMPONENTS) { 1010 throw new IllegalArgumentException("result must have length 16"); 1011 } 1012 if (jacobianX != null && (jacobianX.getRows() != STATE_WITH_POSITION_AND_ROTATION_ADJUSTMENT_COMPONENTS 1013 || jacobianX.getColumns() != STATE_WITH_POSITION_AND_ROTATION_ADJUSTMENT_COMPONENTS)) { 1014 throw new IllegalArgumentException("jacobian wrt x must be 16x16"); 1015 } 1016 if (jacobianU != null && (jacobianU.getRows() != STATE_WITH_POSITION_AND_ROTATION_ADJUSTMENT_COMPONENTS 1017 || jacobianU.getColumns() != CONTROL_WITH_POSITION_AND_ROTATION_ADJUSTMENT_COMPONENTS)) { 1018 throw new IllegalArgumentException("jacobian wrt u must be 16x13"); 1019 } 1020 1021 // position 1022 final var r = new InhomogeneousPoint3D(x[0], x[1], x[2]); 1023 1024 // orientation 1025 var q = new Quaternion(x[3], x[4], x[5], x[6]); 1026 1027 // linear velocity 1028 var vx = x[7]; 1029 var vy = x[8]; 1030 var vz = x[9]; 1031 1032 // linear acceleration 1033 var ax = x[10]; 1034 var ay = x[11]; 1035 var az = x[12]; 1036 1037 // angular velocity 1038 var wx = x[13]; 1039 var wy = x[14]; 1040 var wz = x[15]; 1041 1042 // position change (control) 1043 final var drx = u[0]; 1044 final var dry = u[1]; 1045 final var drz = u[2]; 1046 1047 // rotation change (control) 1048 final var dq = new Quaternion(u[3], u[4], u[5], u[6]); 1049 1050 // linear velocity change (control) 1051 final var uvx = u[7]; 1052 final var uvy = u[8]; 1053 final var uvz = u[9]; 1054 1055 // linear acceleration change (control) 1056 final var uax = u[10]; 1057 final var uay = u[11]; 1058 final var uaz = u[12]; 1059 1060 // angular velocity change (control) 1061 final var uwx = u[13]; 1062 final var uwy = u[14]; 1063 final var uwz = u[15]; 1064 1065 try { 1066 // update velocity 1067 final var vv = new Matrix(SPEED_COMPONENTS, SPEED_COMPONENTS); 1068 final var va = new Matrix(SPEED_COMPONENTS, ACCELERATION_COMPONENTS); 1069 final var v = VelocityPredictor.predict(vx, vy, vz, ax, ay, az, dt, vv, va); 1070 1071 // update position 1072 Matrix rr = null; 1073 Matrix rv = null; 1074 Matrix ra = null; 1075 if (jacobianX != null) { 1076 rr = new Matrix(Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH, 1077 Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH); 1078 rv = new Matrix(Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH, SPEED_COMPONENTS); 1079 ra = new Matrix(Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH, ACCELERATION_COMPONENTS); 1080 } 1081 PositionPredictor.predictWithPositionAdjustment(r, drx, dry, drz, vx, vy, vz, ax, ay, az, dt, r, rr, 1082 null, rv, ra); 1083 1084 // update orientation 1085 Matrix qq = null; 1086 Matrix qdq = null; 1087 Matrix qw = null; 1088 if (jacobianX != null) { 1089 qq = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS); 1090 qw = new Matrix(Quaternion.N_PARAMS, ANGULAR_SPEED_COMPONENTS); 1091 } 1092 if (jacobianU != null) { 1093 qdq = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS); 1094 } 1095 q = QuaternionPredictor.predictWithRotationAdjustment(q, dq, wx, wy, wz, dt, qq, qdq, qw); 1096 1097 // set updated linear velocity 1098 vx = v[0]; 1099 vy = v[1]; 1100 vz = v[2]; 1101 1102 // apply control signals 1103 vx += uvx; 1104 vy += uvy; 1105 vz += uvz; 1106 1107 ax += uax; 1108 ay += uay; 1109 az += uaz; 1110 1111 wx += uwx; 1112 wy += uwy; 1113 wz += uwz; 1114 1115 // set new state 1116 result[0] = r.getInhomX(); 1117 result[1] = r.getInhomY(); 1118 result[2] = r.getInhomZ(); 1119 1120 result[3] = q.getA(); 1121 result[4] = q.getB(); 1122 result[5] = q.getC(); 1123 result[6] = q.getD(); 1124 1125 result[7] = vx; 1126 result[8] = vy; 1127 result[9] = vz; 1128 1129 result[10] = ax; 1130 result[11] = ay; 1131 result[12] = az; 1132 1133 result[13] = wx; 1134 result[14] = wy; 1135 result[15] = wz; 1136 1137 // jacobians 1138 if (jacobianX != null) { 1139 // [Rr 0 Rv Ra 0 ] 1140 // [0 Qq 0 0 Qw ] 1141 // [0 0 Vv Va 0 ] 1142 // [0 0 0 eye 0 ] 1143 // [0 0 0 0 eye] 1144 jacobianX.initialize(0.0); 1145 jacobianX.setSubmatrix(0, 0, 2, 2, rr); 1146 1147 jacobianX.setSubmatrix(3, 3, 6, 6, qq); 1148 1149 jacobianX.setSubmatrix(0, 7, 2, 9, rv); 1150 1151 jacobianX.setSubmatrix(7, 7, 9, 9, vv); 1152 1153 jacobianX.setSubmatrix(0, 10, 2, 12, ra); 1154 1155 jacobianX.setSubmatrix(7, 10, 9, 12, va); 1156 1157 jacobianX.setSubmatrix(3, 13, 6, 15, qw); 1158 1159 for (int i = 10; i < STATE_WITH_POSITION_AND_ROTATION_ADJUSTMENT_COMPONENTS; i++) { 1160 jacobianX.setElementAt(i, i, 1.0); 1161 } 1162 } 1163 1164 if (jacobianU != null) { 1165 jacobianU.initialize(0.0); 1166 1167 // variation of position 1168 for (var i = 0; i < Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH; i++) { 1169 jacobianU.setElementAt(i, i, 1.0); 1170 } 1171 1172 // variation of rotation 1173 jacobianU.setSubmatrix(3, 3, 6, 6, qdq); 1174 1175 // variation of linear speed and acceleration, and angular speed 1176 for (var i = 7; i < STATE_WITH_POSITION_AND_ROTATION_ADJUSTMENT_COMPONENTS; i++) { 1177 jacobianU.setElementAt(i, i, 1.0); 1178 } 1179 } 1180 } catch (final WrongSizeException ignore) { 1181 // never thrown 1182 } 1183 } 1184 1185 /** 1186 * Updates the system model (position, orientation, linear velocity, 1187 * linear acceleration and angular velocity) assuming a constant 1188 * acceleration model when no acceleration or velocity control signal is 1189 * present. 1190 * 1191 * @param x initial system state containing: position-x, position-y, 1192 * position-z, quaternion-a, quaternion-b, quaternion-c, quaternion-d, 1193 * linear-velocity-x, linear-velocity-y, linear-velocity-z, 1194 * linear-acceleration-x, linear-acceleration-y, linear-acceleration-z, 1195 * angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have 1196 * length 16. 1197 * @param u perturbations or control signals: position-change-x, 1198 * position-change-y, position-change-z, quaternion-change-a, 1199 * quaternion-change-b, quaternion-change-c, quaternion-change-d, 1200 * linear-velocity-change-x, linear-velocity-change-y, 1201 * linear-velocity-change-z, linear-acceleration-change-x, 1202 * linear-acceleration-change-y, linear-acceleration-change-z, 1203 * angular-velocity-change-x, angular-velocity-change-y, 1204 * angular-velocity-change-z. Must have length 16. 1205 * @param dt time interval to compute prediction expressed in seconds. 1206 * @param result instance where updated system model will be stored. Must 1207 * have length 16. 1208 * @throws IllegalArgumentException if system state array, control array or 1209 * result array do not have proper size. 1210 */ 1211 public static void predictWithPositionAndRotationAdjustment( 1212 final double[] x, final double[] u, final double dt, final double[] result) { 1213 predictWithPositionAndRotationAdjustment(x, u, dt, result, null, null); 1214 } 1215 1216 /** 1217 * Updates the system model (position, orientation, linear velocity, 1218 * linear acceleration and angular velocity) assuming a constant 1219 * acceleration model when no acceleration or velocity control signal is 1220 * present. 1221 * 1222 * @param x initial system state containing: position-x, position-y, 1223 * position-z, quaternion-a, quaternion-b, quaternion-c, quaternion-d, 1224 * linear-velocity-x, linear-velocity-y, linear-velocity-z, 1225 * linear-acceleration-x, linear-acceleration-y, linear-acceleration-z, 1226 * angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have 1227 * length 16. 1228 * @param u perturbations or control signals: position-change-x, 1229 * position-change-y, position-change-z, quaternion-change-a, 1230 * quaternion-change-b, quaternion-change-c, quaternion-change-d, 1231 * linear-velocity-change-x, linear-velocity-change-y, 1232 * linear-velocity-change-z, linear-acceleration-change-x, 1233 * linear-acceleration-change-y, linear-acceleration-change-z, 1234 * angular-velocity-change-x, angular-velocity-change-y, 1235 * angular-velocity-change-z. Must have length 16. 1236 * @param dt time interval to compute prediction expressed in seconds. 1237 * @param jacobianX jacobian wrt system state. Must be 16x16. 1238 * @param jacobianU jacobian wrt control. must be 16x16. 1239 * @return a new array containing updated system model. 1240 * @throws IllegalArgumentException if system state array, control array or 1241 * jacobians do not have proper size. 1242 */ 1243 public static double[] predictWithPositionAndRotationAdjustment( 1244 final double[] x, final double[] u, final double dt, final Matrix jacobianX, final Matrix jacobianU) { 1245 final var result = new double[STATE_WITH_POSITION_AND_ROTATION_ADJUSTMENT_COMPONENTS]; 1246 predictWithPositionAndRotationAdjustment(x, u, dt, result, jacobianX, jacobianU); 1247 return result; 1248 } 1249 1250 /** 1251 * Updates the system model (position, orientation, linear velocity, 1252 * linear acceleration and angular velocity) assuming a constant 1253 * acceleration model when no acceleration or velocity control signal is 1254 * present. 1255 * 1256 * @param x initial system state containing: position-x, position-y, 1257 * position-z, quaternion-a, quaternion-b, quaternion-c, quaternion-d, 1258 * linear-velocity-x, linear-velocity-y, linear-velocity-z, 1259 * linear-acceleration-x, linear-acceleration-y, linear-acceleration-z, 1260 * angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have 1261 * length 16. 1262 * @param u perturbations or control signals: position-change-x, 1263 * position-change-y, position-change-z, quaternion-change-a, 1264 * quaternion-change-b, quaternion-change-c, quaternion-change-d, 1265 * linear-velocity-change-x, linear-velocity-change-y, 1266 * linear-velocity-change-z, linear-acceleration-change-x, 1267 * linear-acceleration-change-y, linear-acceleration-change-z, 1268 * angular-velocity-change-x, angular-velocity-change-y, 1269 * angular-velocity-change-z. Must have length 16. 1270 * @param dt time interval to compute prediction expressed in seconds. 1271 * @return a new array containing updated system model. 1272 * @throws IllegalArgumentException if system state array, or control array 1273 * do not have proper size. 1274 */ 1275 public static double[] predictWithPositionAndRotationAdjustment( 1276 final double[] x, final double[] u, final double dt) { 1277 final var result = new double[STATE_WITH_POSITION_AND_ROTATION_ADJUSTMENT_COMPONENTS]; 1278 predictWithPositionAndRotationAdjustment(x, u, dt, result); 1279 return result; 1280 } 1281 }