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.geometry.InhomogeneousPoint3D; 20 import com.irurueta.geometry.Point3D; 21 22 /** 23 * Utility class to predict position of device. 24 */ 25 @SuppressWarnings("DuplicatedCode") 26 public class PositionPredictor { 27 28 /** 29 * Number of components of speed. 30 */ 31 public static final int SPEED_COMPONENTS = 3; 32 33 /** 34 * Number of components of acceleration. 35 */ 36 public static final int ACCELERATION_COMPONENTS = 3; 37 38 /** 39 * Constructor. 40 */ 41 private PositionPredictor() { 42 } 43 44 /** 45 * Predicts the updated position. 46 * 47 * @param r current position. Expressed in meters. 48 * @param vx velocity in x-axis. Expressed in m/s. 49 * @param vy velocity in y-axis. Expressed in m/s. 50 * @param vz velocity in z-axis. Expressed in m/s. 51 * @param ax acceleration in x-axis. Expressed in m/s^2. 52 * @param ay acceleration in y-axis. Expressed in m/s^2. 53 * @param az acceleration in z-axis. Expressed in m/s^2. 54 * @param dt time interval to compute prediction expressed in seconds. 55 * @param result instance where updated position is stored. 56 * @param jacobianR jacobian wrt position. Must be 3x3. 57 * @param jacobianV jacobian wrt speed. Must be 3x3. 58 * @param jacobianA jacobian wrt acceleration. Must be 3x3. 59 * @throws IllegalArgumentException if any of provided jacobians does not 60 * have proper size. 61 * @see <a href="https://github.com/joansola/slamtb">rpredict.m at https://github.com/joansola/slamtb</a> 62 */ 63 public static void predict( 64 final InhomogeneousPoint3D r, final double vx, final double vy, final double vz, 65 final double ax, final double ay, final double az, final double dt, 66 final InhomogeneousPoint3D result, final Matrix jacobianR, 67 final Matrix jacobianV, final Matrix jacobianA) { 68 if (jacobianR != null && (jacobianR.getRows() != Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH 69 || jacobianR.getColumns() != Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH)) { 70 throw new IllegalArgumentException("jacobian wrt r must be 3x3"); 71 } 72 if (jacobianV != null && (jacobianV.getRows() != Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH 73 || jacobianV.getColumns() != SPEED_COMPONENTS)) { 74 throw new IllegalArgumentException( 75 "jacobian wrt speed must be 3x3"); 76 } 77 if (jacobianA != null && (jacobianA.getRows() != Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH 78 || jacobianA.getColumns() != ACCELERATION_COMPONENTS)) { 79 throw new IllegalArgumentException( 80 "jacobian wrt acceleration must be 3x3"); 81 } 82 83 if (jacobianR != null) { 84 // set to the identity 85 jacobianR.initialize(0.0); 86 jacobianR.setElementAt(0, 0, 1.0); 87 jacobianR.setElementAt(1, 1, 1.0); 88 jacobianR.setElementAt(2, 2, 1.0); 89 } 90 if (jacobianV != null) { 91 // identity multiplied by dt 92 jacobianV.initialize(0.0); 93 jacobianV.setElementAt(0, 0, dt); 94 jacobianV.setElementAt(1, 1, dt); 95 jacobianV.setElementAt(2, 2, dt); 96 } 97 98 99 if (ax == 0.0 && ay == 0.0 && az == 0.0) { 100 result.setCoordinates( 101 r.getInhomX() + vx * dt, 102 r.getInhomY() + vy * dt, 103 r.getInhomZ() + vz * dt); 104 105 if (jacobianA != null) { 106 jacobianA.initialize(0.0); 107 } 108 } else { 109 result.setCoordinates( 110 r.getInhomX() + vx * dt + 0.5 * ax * dt * dt, 111 r.getInhomY() + vy * dt + 0.5 * ay * dt * dt, 112 r.getInhomZ() + vz * dt + 0.5 * az * dt * dt); 113 114 if (jacobianA != null) { 115 // identity multiplied by 0.5*dt^2 116 final var value = 0.5 * dt * dt; 117 jacobianA.initialize(0.0); 118 jacobianA.setElementAt(0, 0, value); 119 jacobianA.setElementAt(1, 1, value); 120 jacobianA.setElementAt(2, 2, value); 121 } 122 } 123 } 124 125 /** 126 * Predicts the updated position. 127 * 128 * @param r current position. Expressed in meters. 129 * @param vx velocity in x-axis. Expressed in m/s. 130 * @param vy velocity in y-axis. Expressed in m/s. 131 * @param vz velocity in z-axis. Expressed in m/s. 132 * @param ax acceleration in x-axis. Expressed in m/s^2. 133 * @param ay acceleration in y-axis. Expressed in m/s^2. 134 * @param az acceleration in z-axis. Expressed in m/s^2. 135 * @param dt time interval to compute prediction expressed in seconds. 136 * @param result instance where updated position is stored. 137 * @see <a href="https://github.com/joansola/slamtb">rpredict.m at https://github.com/joansola/slamtb</a> 138 */ 139 public static void predict( 140 final InhomogeneousPoint3D r, final double vx, final double vy, final double vz, 141 final double ax, final double ay, final double az, final double dt, 142 final InhomogeneousPoint3D result) { 143 predict(r, vx, vy, vz, ax, ay, az, dt, result, null, null, null); 144 } 145 146 /** 147 * Predicts the updated position. 148 * 149 * @param r point containing current position in 3D. Expressed in meters. 150 * @param v array containing 3 components of velocity. Expressed in m/s. 151 * Must have length 3. 152 * @param a array containing 3 components of acceleration. Expressed in 153 * m/s^2. Must have length 3. 154 * @param dt time interval to compute prediction expressed in seconds. 155 * @param result instance where updated position is stored. 156 * @param jacobianR jacobian wrt position. Must be 3x3. 157 * @param jacobianV jacobian wrt speed. Must be 3x3. 158 * @param jacobianA jacobian wrt acceleration. Must be 3x3. 159 * @throws IllegalArgumentException if any of provided jacobians does not 160 * have proper size of velocity or acceleration do not have length 3. 161 * @see <a href="https://github.com/joansola/slamtb">rpredict.m at https://github.com/joansola/slamtb</a> 162 */ 163 public static void predict( 164 final InhomogeneousPoint3D r, final double[] v, final double[] a, final double dt, 165 final InhomogeneousPoint3D result, final Matrix jacobianR, final Matrix jacobianV, 166 final Matrix jacobianA) { 167 if (v.length != SPEED_COMPONENTS) { 168 // v must have length 3 169 throw new IllegalArgumentException(); 170 } 171 if (a.length != ACCELERATION_COMPONENTS) { 172 // a must have length 3 173 throw new IllegalArgumentException(); 174 } 175 predict(r, v[0], v[1], v[2], a[0], a[1], a[2], dt, result, jacobianR, jacobianV, jacobianA); 176 } 177 178 /** 179 * Predicts the updated position. 180 * 181 * @param r point containing current position in 3D. Expressed in meters. 182 * @param v array containing 3 components of velocity. Expressed in m/s. 183 * Must have length 3. 184 * @param a array containing 3 components of acceleration. Expressed in 185 * m/s^2. Must have length 3. 186 * @param dt time interval to compute prediction expressed in seconds. 187 * @param result instance where updated position is stored. 188 * @see <a href="https://github.com/joansola/slamtb">rpredict.m at https://github.com/joansola/slamtb</a> 189 */ 190 public static void predict( 191 final InhomogeneousPoint3D r, final double[] v, final double[] a, final double dt, 192 final InhomogeneousPoint3D result) { 193 predict(r, v, a, dt, result, null, null, null); 194 } 195 196 /** 197 * Predicts the updated position assuming no acceleration. 198 * 199 * @param r current position. Expressed in meters. 200 * @param vx velocity in x-axis. Expressed in m/s. 201 * @param vy velocity in y-axis. Expressed in m/s. 202 * @param vz velocity in z-axis. Expressed in m/s. 203 * @param dt time interval to compute prediction expressed in seconds. 204 * @param result instance where updated position is stored. 205 * @param jacobianR jacobian wrt position. Must be 3x3. 206 * @param jacobianV jacobian wrt speed. Must be 3x3. 207 * @param jacobianA jacobian wrt acceleration. Must be 3x3. 208 * @throws IllegalArgumentException if any of provided jacobians does not 209 * have proper size. 210 * @see <a href="https://github.com/joansola/slamtb">rpredict.m at https://github.com/joansola/slamtb</a> 211 */ 212 public static void predict( 213 final InhomogeneousPoint3D r, final double vx, final double vy, final double vz, final double dt, 214 final InhomogeneousPoint3D result, final Matrix jacobianR, final Matrix jacobianV, final Matrix jacobianA) { 215 predict(r, vx, vy, vz, 0.0, 0.0, 0.0, dt, result, jacobianR, jacobianV, jacobianA); 216 } 217 218 /** 219 * Predicts the updated position assuming no acceleration. 220 * 221 * @param r current position. Expressed in meters. 222 * @param vx velocity in x-axis. Expressed in m/s. 223 * @param vy velocity in y-axis. Expressed in m/s. 224 * @param vz velocity in z-axis. Expressed in m/s. 225 * @param dt time interval to compute prediction expressed in seconds. 226 * @param result instance where updated position is stored. 227 * @see <a href="https://github.com/joansola/slamtb">rpredict.m at https://github.com/joansola/slamtb</a> 228 */ 229 public static void predict( 230 final InhomogeneousPoint3D r, final double vx, final double vy, final double vz, final double dt, 231 final InhomogeneousPoint3D result) { 232 predict(r, vx, vy, vz, dt, result, null, null, null); 233 } 234 235 /** 236 * Predicts the updated position assuming no acceleration. 237 * 238 * @param r point containing current position in 3D. Expressed in meters. 239 * @param v array containing 3 components of velocity. Expressed in m/s. 240 * Must have length 3. 241 * @param dt time interval to compute prediction expressed in seconds. 242 * @param result instance where updated position is stored. 243 * @param jacobianR jacobian wrt position. Must be 3x3. 244 * @param jacobianV jacobian wrt speed. Must be 3x3. 245 * @param jacobianA jacobian wrt acceleration. Must be 3x3. 246 * @throws IllegalArgumentException if any of provided jacobians does not 247 * have proper size of velocity does not have length 3. 248 * @see <a href="https://github.com/joansola/slamtb">rpredict.m at https://github.com/joansola/slamtb</a> 249 */ 250 public static void predict( 251 final InhomogeneousPoint3D r, final double[] v, final double dt, final InhomogeneousPoint3D result, 252 final Matrix jacobianR, final Matrix jacobianV, final Matrix jacobianA) { 253 if (v.length != SPEED_COMPONENTS) { 254 throw new IllegalArgumentException("v must have length 3"); 255 } 256 predict(r, v[0], v[1], v[2], dt, result, jacobianR, jacobianV, jacobianA); 257 } 258 259 /** 260 * Predicts the updated position assuming no acceleration. 261 * 262 * @param r point containing current position in 3D. Expressed in meters. 263 * @param v array containing 3 components of velocity. Expressed in m/s. 264 * Must have length 3. 265 * @param dt time interval to compute prediction expressed in seconds. 266 * @param result instance where updated position is stored. 267 * @throws IllegalArgumentException if velocity array does not have length 268 * 3. 269 * @see <a href="https://github.com/joansola/slamtb">rpredict.m at https://github.com/joansola/slamtb</a> 270 */ 271 public static void predict( 272 final InhomogeneousPoint3D r, final double[] v, final double dt, final InhomogeneousPoint3D result) { 273 predict(r, v, dt, result, null, null, null); 274 } 275 276 /** 277 * Predicts the updated position 278 * 279 * @param r current position. Expressed in meters. 280 * @param vx velocity in x-axis. Expressed in m/s. 281 * @param vy velocity in y-axis. Expressed in m/s. 282 * @param vz velocity in z-axis. Expressed in m/s. 283 * @param ax acceleration in x-axis. Expressed in m/s^2. 284 * @param ay acceleration in y-axis. Expressed in m/s^2. 285 * @param az acceleration in z-axis. Expressed in m/s^2. 286 * @param dt time interval to compute prediction expressed in seconds. 287 * @param jacobianR jacobian wrt position. Must be 3x3. 288 * @param jacobianV jacobian wrt speed. Must be 3x3. 289 * @param jacobianA jacobian wrt acceleration. Must be 3x3. 290 * @return a new instance containing the updated position. 291 * @throws IllegalArgumentException if any of provided jacobians does not 292 * have proper size. 293 * @see <a href="https://github.com/joansola/slamtb">rpredict.m at https://github.com/joansola/slamtb</a> 294 */ 295 public static InhomogeneousPoint3D predict( 296 final InhomogeneousPoint3D r, final double vx, final double vy, final double vz, 297 final double ax, final double ay, final double az, final double dt, 298 final Matrix jacobianR, final Matrix jacobianV, final Matrix jacobianA) { 299 final var result = new InhomogeneousPoint3D(); 300 predict(r, vx, vy, vz, ax, ay, az, dt, result, jacobianR, jacobianV, jacobianA); 301 return result; 302 } 303 304 /** 305 * Predicts the updated position. 306 * 307 * @param r current position. Expressed in meters. 308 * @param vx velocity in x-axis. Expressed in m/s. 309 * @param vy velocity in y-axis. Expressed in m/s. 310 * @param vz velocity in z-axis. Expressed in m/s. 311 * @param ax acceleration in x-axis. Expressed in m/s^2. 312 * @param ay acceleration in y-axis. Expressed in m/s^2. 313 * @param az acceleration in z-axis. Expressed in m/s^2. 314 * @param dt time interval to compute prediction expressed in seconds. 315 * @return a new instance containing the updated position. 316 * @see <a href="https://github.com/joansola/slamtb">rpredict.m at https://github.com/joansola/slamtb</a> 317 */ 318 public static InhomogeneousPoint3D predict( 319 final InhomogeneousPoint3D r, final double vx, final double vy, final double vz, 320 final double ax, final double ay, final double az, final double dt) { 321 final var result = new InhomogeneousPoint3D(); 322 predict(r, vx, vy, vz, ax, ay, az, dt, result); 323 return result; 324 } 325 326 /** 327 * Predicts the updated position. 328 * 329 * @param r point containing current position in 3D. Expressed in meters. 330 * @param v array containing 3 components of velocity. Expressed in m/s. 331 * Must have length 3. 332 * @param a array containing 3 components of acceleration. Expressed in 333 * m/s^2. Must have length 3. 334 * @param dt time interval to compute prediction expressed in seconds. 335 * @param jacobianR jacobian wrt position. Must be 3x3. 336 * @param jacobianV jacobian wrt speed. Must be 3x3. 337 * @param jacobianA jacobian wrt acceleration. Must be 3x3. 338 * @return a new instance containing the updated position. 339 * @throws IllegalArgumentException if any of provided jacobians does not 340 * have proper size. 341 * @see <a href="https://github.com/joansola/slamtb">rpredict.m at https://github.com/joansola/slamtb</a> 342 */ 343 public static InhomogeneousPoint3D predict( 344 final InhomogeneousPoint3D r, final double[] v, final double[] a, final double dt, 345 final Matrix jacobianR, final Matrix jacobianV, final Matrix jacobianA) { 346 final var result = new InhomogeneousPoint3D(); 347 predict(r, v, a, dt, result, jacobianR, jacobianV, jacobianA); 348 return result; 349 } 350 351 /** 352 * Predicts the updated position. 353 * 354 * @param r point containing current position in 3D. Expressed in meters. 355 * @param v array containing 3 components of velocity. Expressed in m/s. 356 * Must have length 3. 357 * @param a array containing 3 components of acceleration. Expressed in 358 * m/s^2. Must have length 3. 359 * @param dt time interval to compute prediction expressed in seconds. 360 * @return a new instance containing the updated position. 361 * @see <a href="https://github.com/joansola/slamtb">rpredict.m at https://github.com/joansola/slamtb</a> 362 */ 363 public static InhomogeneousPoint3D predict( 364 final InhomogeneousPoint3D r, final double[] v, final double[] a, final double dt) { 365 final var result = new InhomogeneousPoint3D(); 366 predict(r, v, a, dt, result); 367 return result; 368 } 369 370 /** 371 * Predicts the updated position assuming no acceleration. 372 * 373 * @param r current position. Expressed in meters. 374 * @param vx velocity in x-axis. Expressed in m/s. 375 * @param vy velocity in y-axis. Expressed in m/s. 376 * @param vz velocity in z-axis. Expressed in m/s. 377 * @param dt time interval to compute prediction expressed in seconds. 378 * @param jacobianR jacobian wrt position. Must be 3x3. 379 * @param jacobianV jacobian wrt speed. Must be 3x3. 380 * @param jacobianA jacobian wrt acceleration. Must be 3x3. 381 * @return a new instance containing the updated position. 382 * @throws IllegalArgumentException if any of provided jacobians does not 383 * have proper size. 384 * @see <a href="https://github.com/joansola/slamtb">rpredict.m at https://github.com/joansola/slamtb</a> 385 */ 386 public static InhomogeneousPoint3D predict( 387 final InhomogeneousPoint3D r, final double vx, final double vy, final double vz, 388 final double dt, final Matrix jacobianR, final Matrix jacobianV, final Matrix jacobianA) { 389 final var result = new InhomogeneousPoint3D(); 390 predict(r, vx, vy, vz, dt, result, jacobianR, jacobianV, jacobianA); 391 return result; 392 } 393 394 /** 395 * Predicts the updated position assuming no acceleration. 396 * 397 * @param r current position. Expressed in meters. 398 * @param vx velocity in x-axis. Expressed in m/s. 399 * @param vy velocity in y-axis. Expressed in m/s. 400 * @param vz velocity in z-axis. Expressed in m/s. 401 * @param dt time interval to compute prediction expressed in seconds. 402 * @return a new instance containing the updated position. 403 * @see <a href="https://github.com/joansola/slamtb">rpredict.m at https://github.com/joansola/slamtb</a> 404 */ 405 public static InhomogeneousPoint3D predict( 406 final InhomogeneousPoint3D r, final double vx, final double vy, final double vz, final double dt) { 407 final var result = new InhomogeneousPoint3D(); 408 predict(r, vx, vy, vz, dt, result); 409 return result; 410 } 411 412 /** 413 * Predicts the updated position assuming no acceleration. 414 * 415 * @param r current position. Expressed in meters. 416 * @param v array containing 3 components of velocity. Expressed in m/s. 417 * Must have length 3. 418 * @param dt time interval to compute prediction expressed in seconds. 419 * @param jacobianR jacobian wrt position. Must be 3x3. 420 * @param jacobianV jacobian wrt speed. Must be 3x3. 421 * @param jacobianA jacobian wrt acceleration. Must be 3x3. 422 * @return a new instance containing the updated position. 423 * @throws IllegalArgumentException if any of provided jacobians does not 424 * have proper size of velocity does not have length 3. 425 * @see <a href="https://github.com/joansola/slamtb">rpredict.m at https://github.com/joansola/slamtb</a> 426 */ 427 public static InhomogeneousPoint3D predict( 428 final InhomogeneousPoint3D r, final double[] v, final double dt, 429 final Matrix jacobianR, final Matrix jacobianV, final Matrix jacobianA) { 430 final var result = new InhomogeneousPoint3D(); 431 predict(r, v, dt, result, jacobianR, jacobianV, jacobianA); 432 return result; 433 } 434 435 /** 436 * Predicts the updated position assuming no acceleration. 437 * 438 * @param r current position. Expressed in meters. 439 * @param v array containing 3 components of velocity. Expressed in m/s. 440 * Must have length 3. 441 * @param dt time interval to compute prediction expressed in seconds. 442 * @return a new instance containing the updated position. 443 * @throws IllegalArgumentException if size of v array is not 3. 444 * @see <a href="https://github.com/joansola/slamtb">rpredict.m at https://github.com/joansola/slamtb</a> 445 */ 446 public static InhomogeneousPoint3D predict( 447 final InhomogeneousPoint3D r, final double[] v, final double dt) { 448 final var result = new InhomogeneousPoint3D(); 449 predict(r, v, dt, result); 450 return result; 451 } 452 453 /** 454 * Predicts the updated position by using provided position variation, 455 * current speed and current acceleration. 456 * 457 * @param r current position. Expressed in meters. 458 * @param drx adjustment of position in x-axis. Expressed in meters. 459 * @param dry adjustment of position in y-axis. Expressed in meters. 460 * @param drz adjustment of position in z-axis. Expressed in meters. 461 * @param vx velocity in x-axis. Expressed in m/s. 462 * @param vy velocity in y-axis. Expressed in m/s. 463 * @param vz velocity in z-axis. Expressed in m/s. 464 * @param ax acceleration in x-axis. Expressed in m/s^2. 465 * @param ay acceleration in y-axis. Expressed in m/s^2. 466 * @param az acceleration in z-axis. Expressed in m/s^2. 467 * @param dt time interval to compute prediction expressed in seconds. 468 * @param result instance where updated position is stored. 469 * @param jacobianR jacobian wrt position. Must be 3x3. 470 * @param jacobianDR jacobian wrt position adjustment. Must be 3x3. 471 * @param jacobianV jacobian wrt speed. Must be 3c3. 472 * @param jacobianA jacobian wrt acceleration. Must be 3x3. 473 * @throws IllegalArgumentException if any of provided jacobians does not 474 * have proper size. 475 */ 476 public static void predictWithPositionAdjustment( 477 final InhomogeneousPoint3D r, final double drx, final double dry, final double drz, 478 final double vx, final double vy, final double vz, 479 final double ax, final double ay, final double az, final double dt, 480 final InhomogeneousPoint3D result, final Matrix jacobianR, final Matrix jacobianDR, 481 final Matrix jacobianV, final Matrix jacobianA) { 482 if (jacobianR != null && (jacobianR.getRows() != Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH 483 || jacobianR.getColumns() != Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH)) { 484 throw new IllegalArgumentException("jacobian wrt r must be 3x3"); 485 } 486 if (jacobianDR != null && (jacobianDR.getRows() != Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH 487 || jacobianDR.getColumns() != Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH)) { 488 throw new IllegalArgumentException("jacobian wrt dr must be 3x3"); 489 } 490 if (jacobianV != null && (jacobianV.getRows() != Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH 491 || jacobianV.getColumns() != SPEED_COMPONENTS)) { 492 throw new IllegalArgumentException("jacobian wrt speed must be 3x3"); 493 } 494 if (jacobianA != null && (jacobianA.getRows() != Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH 495 || jacobianA.getColumns() != ACCELERATION_COMPONENTS)) { 496 throw new IllegalArgumentException( 497 "jacobian wrt acceleration must be 3x3"); 498 } 499 500 if (jacobianR != null) { 501 // set to identity 502 jacobianR.initialize(0.0); 503 jacobianR.setElementAt(0, 0, 1.0); 504 jacobianR.setElementAt(1, 1, 1.0); 505 jacobianR.setElementAt(2, 2, 1.0); 506 } 507 if (jacobianDR != null) { 508 // set to identity 509 jacobianDR.initialize(0.0); 510 jacobianDR.setElementAt(0, 0, 1.0); 511 jacobianDR.setElementAt(1, 1, 1.0); 512 jacobianDR.setElementAt(2, 2, 1.0); 513 } 514 if (jacobianV != null) { 515 // identity multiplied by dt 516 jacobianV.initialize(0.0); 517 jacobianV.setElementAt(0, 0, dt); 518 jacobianV.setElementAt(1, 1, dt); 519 jacobianV.setElementAt(2, 2, dt); 520 } 521 522 if (ax == 0.0 && ay == 0.0 && az == 0.0) { 523 result.setCoordinates( 524 r.getInhomX() + drx + vx * dt, 525 r.getInhomY() + dry + vy * dt, 526 r.getInhomZ() + drz + vz * dt); 527 528 if (jacobianA != null) { 529 jacobianA.initialize(0.0); 530 } 531 } else { 532 result.setCoordinates( 533 r.getInhomX() + drx + vx * dt + 0.5 * ax * dt * dt, 534 r.getInhomY() + dry + vy * dt + 0.5 * ay * dt * dt, 535 r.getInhomZ() + drz + vz * dt + 0.5 * az * dt * dt); 536 537 if (jacobianA != null) { 538 // identity multiplied by 0.5*dt^2 539 final var value = 0.5 * dt * dt; 540 jacobianA.initialize(0.0); 541 jacobianA.setElementAt(0, 0, value); 542 jacobianA.setElementAt(1, 1, value); 543 jacobianA.setElementAt(2, 2, value); 544 } 545 } 546 } 547 548 /** 549 * Predicts the updated position by using provided position variation, 550 * current speed and current acceleration. 551 * 552 * @param r current position. Expressed in meters. 553 * @param drx adjustment of position in x-axis. Expressed in meters. 554 * @param dry adjustment of position in y-axis. Expressed in meters. 555 * @param drz adjustment of position in z-axis. Expressed in meters. 556 * @param vx velocity in x-axis. Expressed in m/s. 557 * @param vy velocity in y-axis. Expressed in m/s. 558 * @param vz velocity in z-axis. Expressed in m/s. 559 * @param ax acceleration in x-axis. Expressed in m/s^2. 560 * @param ay acceleration in y-axis. Expressed in m/s^2. 561 * @param az acceleration in z-axis. Expressed in m/s^2. 562 * @param dt time interval to compute prediction expressed in seconds. 563 * @param result instance where updated position is stored. 564 */ 565 public static void predictWithPositionAdjustment( 566 final InhomogeneousPoint3D r, final double drx, final double dry, final double drz, 567 final double vx, final double vy, final double vz, 568 final double ax, final double ay, final double az, final double dt, final InhomogeneousPoint3D result) { 569 predictWithPositionAdjustment(r, drx, dry, drz, vx, vy, vz, ax, ay, az, dt, result, 570 null, null, null, null); 571 } 572 573 /** 574 * Predicts the updated position by using provided position variation, 575 * current speed and current acceleration. 576 * 577 * @param r current position. Expressed in meters. 578 * @param dr adjustment of position (x, y, z). Must have length 3. 579 * @param v array containing 3 components of velocity. Expressed in m/s. 580 * Must have length 3. 581 * @param a array containing 3 components of acceleration. Expressed in 582 * m/s^2. Must have length 3. 583 * @param dt time interval to compute prediction expressed in seconds. 584 * @param result instance where updated position is stored. 585 * @param jacobianR jacobian wrt position. Must be 3x3. 586 * @param jacobianDR jacobian wrt position adjustment. Must be 3x3. 587 * @param jacobianV jacobian wrt speed. Must be 3x3. 588 * @param jacobianA jacobian wrt acceleration. Must be 3x3. 589 * @throws IllegalArgumentException if any of provided jacobians does not 590 * have proper size or dr, v or a do not have length 3. 591 */ 592 public static void predictWithPositionAdjustment( 593 final InhomogeneousPoint3D r, final double[] dr, final double[] v, final double[] a, 594 final double dt, final InhomogeneousPoint3D result, final Matrix jacobianR, 595 final Matrix jacobianDR, final Matrix jacobianV, final Matrix jacobianA) { 596 if (dr.length != Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH) { 597 throw new IllegalArgumentException("dr must have length 3"); 598 } 599 if (v.length != SPEED_COMPONENTS) { 600 throw new IllegalArgumentException("v must have length 3"); 601 } 602 if (a.length != ACCELERATION_COMPONENTS) { 603 throw new IllegalArgumentException("a must have length 3"); 604 } 605 predictWithPositionAdjustment(r, dr[0], dr[1], dr[2], v[0], v[1], v[2], a[0], a[1], a[2], dt, result, 606 jacobianR, jacobianDR, jacobianV, jacobianA); 607 } 608 609 /** 610 * Predicts the updated position. 611 * 612 * @param r point containing current position in 3D. Expressed in meters. 613 * @param dr adjustment of position (x, y, z). Must have length 3. 614 * @param v array containing 3 components of velocity. Expressed in m/s. 615 * Must have length 3. 616 * @param a array containing 3 components of acceleration. Expressed in 617 * m/s^2. Must have length 3. 618 * @param dt time interval to compute prediction expressed in seconds. 619 * @param result instance where updated position is stored. 620 * @throws IllegalArgumentException if dr, v or a do not have length 3. 621 */ 622 public static void predictWithPositionAdjustment( 623 final InhomogeneousPoint3D r, final double[] dr, final double[] v, final double[] a, final double dt, 624 final InhomogeneousPoint3D result) { 625 predictWithPositionAdjustment(r, dr, v, a, dt, result, null, null, null, 626 null); 627 } 628 629 /** 630 * Predicts the updated position by using provided position variation, 631 * current speed and current acceleration. 632 * 633 * @param r current position. Expressed in meters. 634 * @param drx adjustment of position in x-axis. Expressed in meters. 635 * @param dry adjustment of position in y-axis. Expressed in meters. 636 * @param drz adjustment of position in z-axis. Expressed in meters. 637 * @param vx velocity in x-axis. Expressed in m/s. 638 * @param vy velocity in y-axis. Expressed in m/s. 639 * @param vz velocity in z-axis. Expressed in m/s. 640 * @param ax acceleration in x-axis. Expressed in m/s^2. 641 * @param ay acceleration in y-axis. Expressed in m/s^2. 642 * @param az acceleration in z-axis. Expressed in m/s^2. 643 * @param dt time interval to compute prediction expressed in seconds. 644 * @param jacobianR jacobian wrt position. Must be 3x3. 645 * @param jacobianDR jacobian wrt position adjustment. Must be 3x3. 646 * @param jacobianV jacobian wrt speed. Must be 3c3. 647 * @param jacobianA jacobian wrt acceleration. Must be 3x3. 648 * @return a new updated position. 649 * @throws IllegalArgumentException if any of provided jacobians does not 650 * have proper size. 651 */ 652 public static InhomogeneousPoint3D predictWithPositionAdjustment( 653 final InhomogeneousPoint3D r, final double drx, final double dry, final double drz, 654 final double vx, final double vy, final double vz, 655 final double ax, final double ay, final double az, 656 final double dt, final Matrix jacobianR, final Matrix jacobianDR, final Matrix jacobianV, 657 final Matrix jacobianA) { 658 final var result = new InhomogeneousPoint3D(); 659 predictWithPositionAdjustment(r, drx, dry, drz, vx, vy, vz, ax, ay, az, dt, result, 660 jacobianR, jacobianDR, jacobianV, jacobianA); 661 return result; 662 } 663 664 /** 665 * Predicts the updated position by using provided position variation, 666 * current speed and current acceleration. 667 * 668 * @param r current position. Expressed in meters. 669 * @param drx adjustment of position in x-axis. Expressed in meters. 670 * @param dry adjustment of position in y-axis. Expressed in meters. 671 * @param drz adjustment of position in z-axis. Expressed in meters. 672 * @param vx velocity in x-axis. Expressed in m/s. 673 * @param vy velocity in y-axis. Expressed in m/s. 674 * @param vz velocity in z-axis. Expressed in m/s. 675 * @param ax acceleration in x-axis. Expressed in m/s^2. 676 * @param ay acceleration in y-axis. Expressed in m/s^2. 677 * @param az acceleration in z-axis. Expressed in m/s^2. 678 * @param dt time interval to compute prediction expressed in seconds. 679 * @return a new updated position. 680 */ 681 public static InhomogeneousPoint3D predictWithPositionAdjustment( 682 final InhomogeneousPoint3D r, final double drx, final double dry, final double drz, 683 final double vx, final double vy, final double vz, final double ax, final double ay, final double az, 684 final double dt) { 685 final var result = new InhomogeneousPoint3D(); 686 predictWithPositionAdjustment(r, drx, dry, drz, vx, vy, vz, ax, ay, az, dt, result); 687 return result; 688 } 689 690 /** 691 * Predicts the updated position by using provided position variation, 692 * current speed and current acceleration. 693 * 694 * @param r current position. Expressed in meters. 695 * @param dr adjustment of position (x, y, z). Must have length 3. 696 * @param v array containing 3 components of velocity. Expressed in m/s. 697 * Must have length 3. 698 * @param a array containing 3 components of acceleration. Expressed in 699 * m/s^2. Must have length 3. 700 * @param dt time interval to compute prediction expressed in seconds. 701 * @param jacobianR jacobian wrt position. Must be 3x3. 702 * @param jacobianDR jacobian wrt position adjustment. Must be 3x3. 703 * @param jacobianV jacobian wrt speed. Must be 3x3. 704 * @param jacobianA jacobian wrt acceleration. Must be 3x3. 705 * @return a new updated position. 706 * @throws IllegalArgumentException if any of provided jacobians does not 707 * have proper size or dr, v or a do not have length 3. 708 */ 709 public static InhomogeneousPoint3D predictWithPositionAdjustment( 710 final InhomogeneousPoint3D r, final double[] dr, final double[] v, final double[] a, 711 final double dt, final Matrix jacobianR, final Matrix jacobianDR, final Matrix jacobianV, 712 final Matrix jacobianA) { 713 final var result = new InhomogeneousPoint3D(); 714 predictWithPositionAdjustment(r, dr, v, a, dt, result, jacobianR, jacobianDR, jacobianV, jacobianA); 715 return result; 716 } 717 718 /** 719 * Predicts the updated position. 720 * 721 * @param r point containing current position in 3D. Expressed in meters. 722 * @param dr adjustment of position (x, y, z). Must have length 3. 723 * @param v array containing 3 components of velocity. Expressed in m/s. 724 * Must have length 3. 725 * @param a array containing 3 components of acceleration. Expressed in 726 * m/s^2. Must have length 3. 727 * @param dt time interval to compute prediction expressed in seconds. 728 * @return a new updated position. 729 */ 730 public static InhomogeneousPoint3D predictWithPositionAdjustment( 731 final InhomogeneousPoint3D r, final double[] dr, final double[] v, final double[] a, final double dt) { 732 final var result = new InhomogeneousPoint3D(); 733 predictWithPositionAdjustment(r, dr, v, a, dt, result); 734 return result; 735 } 736 }