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 20 /** 21 * Utility class to predict velocity of device. 22 */ 23 @SuppressWarnings("DuplicatedCode") 24 public class VelocityPredictor { 25 26 /** 27 * Number of components of speed. 28 */ 29 public static final int SPEED_COMPONENTS = 3; 30 31 /** 32 * Number of components of acceleration. 33 */ 34 public static final int ACCELERATION_COMPONENTS = 3; 35 36 /** 37 * Constructor. 38 */ 39 private VelocityPredictor() { 40 } 41 42 /** 43 * Predicts the updated velocity. 44 * 45 * @param vx current velocity in x-axis. Expressed in m/s. 46 * @param vy current velocity in y-axis. Expressed in m/s. 47 * @param vz current velocity in z-axis. Expressed in m/s. 48 * @param ax acceleration in x-axis. Expressed in m/s^2. 49 * @param ay acceleration in y-axis. Expressed in m/s^2. 50 * @param az acceleration in z-axis. Expressed in m/s^2. 51 * @param dt time interval to compute prediction expressed in seconds. 52 * @param result instance where updated velocity is stored. Must have 53 * length 3. 54 * @param jacobianV jacobian wrt velocity. Must be 3x3. 55 * @param jacobianA jacobian wrt acceleration. Must be 3x3. 56 * @throws IllegalArgumentException if any of provided jacobians does not 57 * have proper size or if result does not have length 3. 58 * @see <a href="https://github.com/joansola/slamtb">vpredict.m at https://github.com/joansola/slamtb</a> 59 */ 60 public static void predict( 61 final double vx, final double vy, final double vz, 62 final double ax, final double ay, final double az, final double dt, 63 final double[] result, final Matrix jacobianV, final Matrix jacobianA) { 64 if (result.length != SPEED_COMPONENTS) { 65 throw new IllegalArgumentException("result must have length 3"); 66 } 67 if (jacobianV != null && (jacobianV.getRows() != SPEED_COMPONENTS || jacobianV.getColumns() != SPEED_COMPONENTS)) { 68 throw new IllegalArgumentException("jacobian wrt velocity must be 3x3"); 69 } 70 if (jacobianA != null && (jacobianA.getRows() != SPEED_COMPONENTS 71 || jacobianA.getColumns() != ACCELERATION_COMPONENTS)) { 72 throw new IllegalArgumentException("jacobian wrt acceleration must be 3x3"); 73 } 74 75 if (jacobianV != null) { 76 // set to the identity 77 jacobianV.initialize(0.0); 78 jacobianV.setElementAt(0, 0, 1.0); 79 jacobianV.setElementAt(1, 1, 1.0); 80 jacobianV.setElementAt(2, 2, 1.0); 81 } 82 83 if (jacobianA != null) { 84 jacobianA.initialize(0.0); 85 jacobianA.setElementAt(0, 0, dt); 86 jacobianA.setElementAt(1, 1, dt); 87 jacobianA.setElementAt(2, 2, dt); 88 } 89 90 result[0] = vx + ax * dt; 91 result[1] = vy + ay * dt; 92 result[2] = vz + az * dt; 93 } 94 95 /** 96 * Predicts the updated velocity. 97 * 98 * @param vx current velocity in x-axis. Expressed in m/s. 99 * @param vy current velocity in y-axis. Expressed in m/s. 100 * @param vz current velocity in z-axis. Expressed in m/s. 101 * @param ax acceleration in x-axis. Expressed in m/s^2. 102 * @param ay acceleration in y-axis. Expressed in m/s^2. 103 * @param az acceleration in z-axis. Expressed in m/s^2. 104 * @param dt time interval to compute prediction expressed in seconds. 105 * @param result instance where updated velocity is stored. 106 * @throws IllegalArgumentException if result does not have length 3. 107 * @see <a href="https://github.com/joansola/slamtb">vpredict.m at https://github.com/joansola/slamtb</a> 108 */ 109 public static void predict( 110 final double vx, final double vy, final double vz, 111 final double ax, final double ay, final double az, final double dt, final double[] result) { 112 predict(vx, vy, vz, ax, ay, az, dt, result, null, null); 113 } 114 115 /** 116 * Predicts the updated velocity. 117 * 118 * @param v array containing 3 components of velocity. Expressed in m/s. 119 * Must have length 3. 120 * @param a array containing 3 components of acceleration. Expressed in 121 * m/s^2. Must have length 3. 122 * @param dt time interval to compute prediction expressed in seconds. 123 * @param result instance where updated velocity is stored. 124 * @param jacobianV jacobian wrt velocity. Must be 3x3. 125 * @param jacobianA jacobian wrt acceleration. Must be 3x3. 126 * @throws IllegalArgumentException if any of provided jacobians does not 127 * have proper size or if v, a or result do not have length 3. 128 * @see <a href="https://github.com/joansola/slamtb">vpredict.m at https://github.com/joansola/slamtb</a> 129 */ 130 public static void predict( 131 final double[] v, final double[] a, final double dt, final double[] result, final Matrix jacobianV, 132 final Matrix jacobianA) { 133 if (v.length != SPEED_COMPONENTS) { 134 throw new IllegalArgumentException("v must have length 3"); 135 } 136 if (a.length != ACCELERATION_COMPONENTS) { 137 throw new IllegalArgumentException("a must have length 3"); 138 } 139 predict(v[0], v[1], v[2], a[0], a[1], a[2], dt, result, jacobianV, jacobianA); 140 } 141 142 /** 143 * Predicts the updated velocity. 144 * 145 * @param v array containing 3 components of velocity. Expressed in m/s. 146 * Must have length 3. 147 * @param a array containing 3 components of acceleration. Expressed in 148 * m/s^2. Must have length 3. 149 * @param dt time interval to compute prediction expressed in seconds. 150 * @param result instance where updated velocity is stored. 151 * @throws IllegalArgumentException if v, a or result do not have length 3. 152 * @see <a href="https://github.com/joansola/slamtb">vpredict.m at https://github.com/joansola/slamtb</a> 153 */ 154 public static void predict(final double[] v, final double[] a, final double dt, final double[] result) { 155 predict(v, a, dt, result, null, null); 156 } 157 158 /** 159 * Predicts the updated velocity. 160 * 161 * @param vx current velocity in x-axis. Expressed in m/s. 162 * @param vy current velocity in y-axis. Expressed in m/s. 163 * @param vz current velocity in z-axis. Expressed in m/s. 164 * @param ax acceleration in x-axis. Expressed in m/s^2. 165 * @param ay acceleration in y-axis. Expressed in m/s^2. 166 * @param az acceleration in z-axis. Expressed in m/s^2. 167 * @param dt time interval to compute prediction expressed in seconds. 168 * @param jacobianV jacobian wrt velocity. Must be 3x3. 169 * @param jacobianA jacobian wrt acceleration. Must be 3x3. 170 * @return a new instance containing updated velocity. 171 * @throws IllegalArgumentException if any of provided jacobians does not 172 * have proper size. 173 * @see <a href="https://github.com/joansola/slamtb">vpredict.m at https://github.com/joansola/slamtb</a> 174 */ 175 public static double[] predict( 176 final double vx, final double vy, final double vz, 177 final double ax, final double ay, final double az, final double dt, final Matrix jacobianV, 178 final Matrix jacobianA) { 179 final var result = new double[SPEED_COMPONENTS]; 180 predict(vx, vy, vz, ax, ay, az, dt, result, jacobianV, jacobianA); 181 return result; 182 } 183 184 /** 185 * Predicts the updated velocity. 186 * 187 * @param vx current velocity in x-axis. Expressed in m/s. 188 * @param vy current velocity in y-axis. Expressed in m/s. 189 * @param vz current velocity in z-axis. Expressed in m/s. 190 * @param ax acceleration in x-axis. Expressed in m/s^2. 191 * @param ay acceleration in y-axis. Expressed in m/s^2. 192 * @param az acceleration in z-axis. Expressed in m/s^2. 193 * @param dt time interval to compute prediction expressed in seconds. 194 * @return a new instance containing updated velocity. 195 * @see <a href="https://github.com/joansola/slamtb">vpredict.m at https://github.com/joansola/slamtb</a> 196 */ 197 public static double[] predict( 198 final double vx, final double vy, final double vz, 199 final double ax, final double ay, final double az, final double dt) { 200 final var result = new double[SPEED_COMPONENTS]; 201 predict(vx, vy, vz, ax, ay, az, dt, result); 202 return result; 203 } 204 205 /** 206 * Predicts the updated velocity. 207 * 208 * @param v array containing 3 components of velocity. Expressed in m/s. 209 * Must have length 3. 210 * @param a array containing 3 components of acceleration. Expressed in 211 * m/s^2. Must have length 3. 212 * @param dt time interval to compute prediction expressed in seconds. 213 * @param jacobianV jacobian wrt velocity. Must be 3x3. 214 * @param jacobianA jacobian wrt acceleration. Must be 3x3. 215 * @return a new instance containing updated velocity. 216 * @throws IllegalArgumentException if any of provided jacobians does not 217 * have proper size or if v or a do not have length 3. 218 * @see <a href="https://github.com/joansola/slamtb">vpredict.m at https://github.com/joansola/slamtb</a> 219 */ 220 public static double[] predict( 221 final double[] v, final double[] a, final double dt, final Matrix jacobianV, final Matrix jacobianA) { 222 final var result = new double[SPEED_COMPONENTS]; 223 predict(v, a, dt, result, jacobianV, jacobianA); 224 return result; 225 } 226 227 /** 228 * Predicts the updated velocity. 229 * 230 * @param v array containing 3 components of velocity. Expressed in m/s. 231 * Must have length 3. 232 * @param a array containing 3 components of acceleration. Expressed in 233 * m/s^2. Must have length 3. 234 * @param dt time interval to compute prediction expressed in seconds. 235 * @return a new instance containing updated velocity. 236 * @throws IllegalArgumentException if a or v do not have length 3. 237 * @see <a href="https://github.com/joansola/slamtb">vpredict.m at https://github.com/joansola/slamtb</a> 238 */ 239 public static double[] predict(final double[] v, final double[] a, final double dt) { 240 final var result = new double[SPEED_COMPONENTS]; 241 predict(v, a, dt, result); 242 return result; 243 } 244 245 /** 246 * Predicts the updated velocity. 247 * 248 * @param vx current velocity in x-axis. Expressed in m/s. 249 * @param vy current velocity in y-axis. Expressed in m/s. 250 * @param vz current velocity in z-axis. Expressed in m/s. 251 * @param dvx velocity adjustment in x-axis. Expressed in m/s. 252 * @param dvy velocity adjustment in y-axis. Expressed in m/s. 253 * @param dvz velocity adjustment in z-axis. Expressed in m/s. 254 * @param ax acceleration in x-axis. Expressed in m/s^2. 255 * @param ay acceleration in y-axis. Expressed in m/s^2. 256 * @param az acceleration in z-axis. Expressed in m/s^2. 257 * @param dt time interval to compute prediction expressed in seconds. 258 * @param result instance where updated velocity is stored. Must have 259 * length 3. 260 * @param jacobianV jacobian wrt velocity. Must be 3x3. 261 * @param jacobianDV jacobian wrt velocity adjustment. Must be 3x3. 262 * @param jacobianA jacobian wrt acceleration. Must be 3x3. 263 * @throws IllegalArgumentException if any of provided jacobians does not 264 * have proper size or if result does not have length 3. 265 */ 266 public static void predictWithVelocityAdjustment( 267 final double vx, final double vy, final double vz, final double dvx, final double dvy, final double dvz, 268 final double ax, final double ay, final double az, final double dt, final double[] result, 269 final Matrix jacobianV, final Matrix jacobianDV, final Matrix jacobianA) { 270 if (result.length != SPEED_COMPONENTS) { 271 throw new IllegalArgumentException("result must have length 3"); 272 } 273 if (jacobianV != null && (jacobianV.getRows() != SPEED_COMPONENTS 274 || jacobianV.getColumns() != SPEED_COMPONENTS)) { 275 throw new IllegalArgumentException("jacobian wrt velocity must be 3x3"); 276 } 277 if (jacobianDV != null && (jacobianDV.getRows() != SPEED_COMPONENTS 278 || jacobianDV.getColumns() != SPEED_COMPONENTS)) { 279 throw new IllegalArgumentException("jacobian wrt velocity variation must be 3x3"); 280 } 281 if (jacobianA != null && (jacobianA.getRows() != SPEED_COMPONENTS 282 || jacobianA.getColumns() != ACCELERATION_COMPONENTS)) { 283 throw new IllegalArgumentException("jacobian wrt acceleration must be 3x3"); 284 } 285 286 if (jacobianV != null) { 287 // set to the identity 288 jacobianV.initialize(0.0); 289 jacobianV.setElementAt(0, 0, 1.0); 290 jacobianV.setElementAt(1, 1, 1.0); 291 jacobianV.setElementAt(2, 2, 1.0); 292 } 293 if (jacobianDV != null) { 294 // set to the identity 295 jacobianDV.initialize(0.0); 296 jacobianDV.setElementAt(0, 0, 1.0); 297 jacobianDV.setElementAt(1, 1, 1.0); 298 jacobianDV.setElementAt(2, 2, 1.0); 299 } 300 if (jacobianA != null) { 301 jacobianA.initialize(0.0); 302 jacobianA.setElementAt(0, 0, dt); 303 jacobianA.setElementAt(1, 1, dt); 304 jacobianA.setElementAt(2, 2, dt); 305 } 306 307 result[0] = vx + dvx + ax * dt; 308 result[1] = vy + dvy + ay * dt; 309 result[2] = vz + dvz + az * dt; 310 } 311 312 /** 313 * Predicts the updated velocity. 314 * 315 * @param vx current velocity in x-axis. Expressed in m/s. 316 * @param vy current velocity in y-axis. Expressed in m/s. 317 * @param vz current velocity in z-axis. Expressed in m/s. 318 * @param dvx velocity adjustment in x-axis. Expressed in m/s. 319 * @param dvy velocity adjustment in y-axis. Expressed in m/s. 320 * @param dvz velocity adjustment in z-axis. Expressed in m/s. 321 * @param ax acceleration in x-axis. Expressed in m/s^2. 322 * @param ay acceleration in y-axis. Expressed in m/s^2. 323 * @param az acceleration in z-axis. Expressed in m/s^2. 324 * @param dt time interval to compute prediction expressed in seconds. 325 * @param result instance where updated velocity is stored. Must have 326 * length 3. 327 * @throws IllegalArgumentException if result does not have length 3. 328 */ 329 public static void predictWithVelocityAdjustment( 330 final double vx, final double vy, final double vz, final double dvx, final double dvy, final double dvz, 331 final double ax, final double ay, final double az, final double dt, final double[] result) { 332 predictWithVelocityAdjustment(vx, vy, vz, dvx, dvy, dvz, ax, ay, az, dt, result, 333 null, null, null); 334 } 335 336 /** 337 * Predicts the updated velocity. 338 * 339 * @param v current velocity (in x,y,z axes). Expressed in m/s. Must have 340 * length 3. 341 * @param dv velocity adjustment (in x,y,z axes). Expressed in m/s. Must 342 * have length 3. 343 * @param a acceleration (in x,y,z axes). Expressed in m/s. Must have length 344 * 3. 345 * @param dt time interval to compute prediction expressed in seconds. 346 * @param result instance where updated velocity is stored. Must have length 347 * 3. 348 * @param jacobianV jacobian wrt velocity. Must be 3x3. 349 * @param jacobianDV jacobian wrt velocity adjustment. Must be 3x3. 350 * @param jacobianA jacobian wrt acceleration. Must be 3x3. 351 * @throws IllegalArgumentException if any of provided jacobians does not 352 * have proper size, or if result "v", "dv" or "a" do not have 353 * length 3. 354 */ 355 public static void predictWithVelocityAdjustment( 356 final double[] v, final double[] dv, final double[] a, final double dt, final double[] result, 357 final Matrix jacobianV, final Matrix jacobianDV, final Matrix jacobianA) { 358 if (v.length != SPEED_COMPONENTS) { 359 throw new IllegalArgumentException("v must have length 3"); 360 } 361 if (dv.length != SPEED_COMPONENTS) { 362 throw new IllegalArgumentException("dv must have length 3"); 363 } 364 if (a.length != ACCELERATION_COMPONENTS) { 365 throw new IllegalArgumentException("a must have length 3"); 366 } 367 predictWithVelocityAdjustment(v[0], v[1], v[2], dv[0], dv[1], dv[2], a[0], a[1], a[2], dt, result, jacobianV, 368 jacobianDV, jacobianA); 369 } 370 371 /** 372 * Predicts the updated velocity. 373 * 374 * @param v current velocity (in x,y,z axes). Expressed in m/s. Must have 375 * length 3. 376 * @param dv velocity adjustment (in x,y,z axes). Expressed in m/s. Must 377 * have length 3. 378 * @param a acceleration (in x,y,z axes). Expressed in m/s. Must have length 379 * 3. 380 * @param dt time interval to compute prediction expressed in seconds. 381 * @param result instance where updated velocity is stored. Must have length 382 * 3. 383 * @throws IllegalArgumentException if v, dv, a or result do not have length 384 * 3. 385 */ 386 public static void predictWithVelocityAdjustment( 387 final double[] v, final double[] dv, final double[] a, final double dt, final double[] result) { 388 predictWithVelocityAdjustment(v, dv, a, dt, result, null, null, null); 389 } 390 391 /** 392 * Predicts the updated velocity. 393 * 394 * @param vx current velocity in x-axis. Expressed in m/s. 395 * @param vy current velocity in y-axis. Expressed in m/s. 396 * @param vz current velocity in z-axis. Expressed in m/s. 397 * @param dvx velocity adjustment in x-axis. Expressed in m/s. 398 * @param dvy velocity adjustment in y-axis. Expressed in m/s. 399 * @param dvz velocity adjustment in z-axis. Expressed in m/s. 400 * @param ax acceleration in x-axis. Expressed in m/s^2. 401 * @param ay acceleration in y-axis. Expressed in m/s^2. 402 * @param az acceleration in z-axis. Expressed in m/s^2. 403 * @param dt time interval to compute prediction expressed in seconds. 404 * @param jacobianV jacobian wrt velocity. Must be 3x3. 405 * @param jacobianDV jacobian wrt velocity adjustment. Must be 3x3. 406 * @param jacobianA jacobian wrt acceleration. Must be 3x3. 407 * @return a new array containing updated velocity. 408 * @throws IllegalArgumentException if any of provided jacobians does not 409 * have proper size. 410 */ 411 public static double[] predictWithVelocityAdjustment( 412 final double vx, final double vy, final double vz, final double dvx, final double dvy, final double dvz, 413 final double ax, final double ay, final double az, final double dt, final Matrix jacobianV, 414 final Matrix jacobianDV, final Matrix jacobianA) { 415 final var result = new double[SPEED_COMPONENTS]; 416 predictWithVelocityAdjustment(vx, vy, vz, dvx, dvy, dvz, ax, ay, az, dt, result, jacobianV, jacobianDV, 417 jacobianA); 418 return result; 419 } 420 421 /** 422 * Predicts the updated velocity. 423 * 424 * @param vx current velocity in x-axis. Expressed in m/s. 425 * @param vy current velocity in y-axis. Expressed in m/s. 426 * @param vz current velocity in z-axis. Expressed in m/s. 427 * @param dvx velocity adjustment in x-axis. Expressed in m/s. 428 * @param dvy velocity adjustment in y-axis. Expressed in m/s. 429 * @param dvz velocity adjustment in z-axis. Expressed in m/s. 430 * @param ax acceleration in x-axis. Expressed in m/s^2. 431 * @param ay acceleration in y-axis. Expressed in m/s^2. 432 * @param az acceleration in z-axis. Expressed in m/s^2. 433 * @param dt time interval to compute prediction expressed in seconds. 434 * @return a new array containing updated velocity. 435 */ 436 public static double[] predictWithVelocityAdjustment( 437 final double vx, final double vy, final double vz, final double dvx, final double dvy, final double dvz, 438 final double ax, final double ay, final double az, final double dt) { 439 final var result = new double[SPEED_COMPONENTS]; 440 predictWithVelocityAdjustment(vx, vy, vz, dvx, dvy, dvz, ax, ay, az, dt, result); 441 return result; 442 } 443 444 /** 445 * Predicts the updated velocity. 446 * 447 * @param v current velocity (in x,y,z axes). Expressed in m/s. Must have 448 * length 3. 449 * @param dv velocity adjustment (in x,y,z axes). Expressed in m/s. Must 450 * have length 3. 451 * @param a acceleration (in x,y,z axes). Expressed in m/s. Must have length 452 * 3. 453 * @param dt time interval to compute prediction expressed in seconds. 454 * @param jacobianV jacobian wrt velocity. Must be 3x3. 455 * @param jacobianDV jacobian wrt velocity adjustment. Must be 3x3. 456 * @param jacobianA jacobian wrt acceleration. Must be 3x3. 457 * @return a new array containing updated velocity. 458 * @throws IllegalArgumentException if any of provided jacobians does not 459 * have proper size, or if "v", "dv" or "a" do not have length 3. 460 */ 461 public static double[] predictWithVelocityAdjustment( 462 final double[] v, final double[] dv, final double[] a, final double dt, final Matrix jacobianV, 463 final Matrix jacobianDV, final Matrix jacobianA) { 464 final var result = new double[SPEED_COMPONENTS]; 465 predictWithVelocityAdjustment(v, dv, a, dt, result, jacobianV, jacobianDV, jacobianA); 466 return result; 467 } 468 469 /** 470 * Predicts the updated velocity. 471 * 472 * @param v current velocity (in x,y,z axes). Expressed in m/s. Must have 473 * length 3. 474 * @param dv velocity adjustment (in x,y,z axes). Expressed in m/s. Must 475 * have length 3. 476 * @param a acceleration (in x,y,z axes). Expressed in m/s. Must have length 477 * 3. 478 * @param dt time interval to compute prediction expressed in seconds. 479 * @return a new array containing updated velocity. 480 * @throws IllegalArgumentException if v, dv or a do not have length 3. 481 */ 482 public static double[] predictWithVelocityAdjustment( 483 final double[] v, final double[] dv, final double[] a, final double dt) { 484 final var result = new double[SPEED_COMPONENTS]; 485 predictWithVelocityAdjustment(v, dv, a, dt, result); 486 return result; 487 } 488 489 }