001package jmri.jmrix.rps; 002 003import edu.umd.cs.findbugs.annotations.SuppressFBWarnings; 004import java.util.Arrays; 005import javax.vecmath.Point3d; 006import org.slf4j.Logger; 007import org.slf4j.LoggerFactory; 008 009/** 010 * Implementation of 2.1th algorithm for reducing Readings 011 * <p> 012 * This algorithm was provided by Robert Ashenfelter based in part on the work 013 * of Ralph Bucher in his paper "Exact Solution for Three Dimensional Hyperbolic 014 * Positioning Algorithm and Synthesizable VHDL Model for Hardware 015 * Implementation". 016 * <p> 017 * Neither Ashenfelter nor Bucher provide any guarantee as to the intellectual 018 * property status of this algorithm. Use it at your own risk. 019 * 020 * <h2>RPSpos2.2 program description.</h2> 021 * 022 * <p> 023 * As in previous versions, the first thing it does is sort the receivers in 024 * order of increasing time delay, discarding those that failed or are too far 025 * or too near, and using the closest ones. There is a maximum that are used, 026 * still set at 50. 027 * <p> 028 * Next it discards those receivers with gross measurement errors. All possible 029 * pairs of receivers are checked to see if the sum of their measured ranges is 030 * less than, or the difference is greater than, the distance between the 031 * receivers. Counts are maintained for each receiver and the one with the 032 * largest count is booted out. The procedure is repeated until there are no 033 * more failures. If fewer than three receivers are left there can be no 034 * solution and an error code is returned. 035 * <p> 036 * Two iterative techniques are used which I call "One-At-A-Time" and 037 * "All-Together." The first looks at one receiver at a time and moves the 038 * estimated position directly toward or away from it such that the distance is 039 * equal to the measured value. This simple technique usually converges quite 040 * rapidly. The second technique accumulates the adjustments for all receivers 041 * and then computes and applies an average for all. It also computes the 042 * variance of the residual errors (differences between measured receiver 043 * distances and those from the computed position) which is used to monitor the 044 * progress of the iterative solution and it implements the rejection of the 045 * measurement with the largest residual when it is deemed to be an outlier. The 046 * second technique is not as fast as the first, but is ultimately more 047 * accurate. 048 * <p> 049 * The solution proceeds in seven stages, much like those in versions 2.0 and 050 * 2.1. Stage 0 does 50 One-At-A-Time iterations with the receivers in the 051 * sorted order. As in previous versions, it starts from a position far, far 052 * below any likely final point. Stage 1 continues the One-At-A-Time iterations 053 * with the receivers in reverse order. Every 50 such iterations the 054 * All-Together procedure is run to check the variance but not to reject 055 * outliers. The One-At-A-Time procedure usually converges in 20-50 iterations, 056 * however for occasional positions the convergence is much slower. The program 057 * moves on to the next stage after a total of 750 iterations, or sooner if it 058 * appears that no further improvement can be had. If the variance indicates 059 * that convergence has been achieved and there are no significant errors, then 060 * the program skips directly to Stage 6. 061 * <p> 062 * Stage 2 is where the outliers are rejected. This is only run when there are 063 * more than 6 receivers; if there are 5 or 6, Stage 3 is run instead; if 4 064 * receivers, Stage 4 (sometimes). By this time a correct, but not particularly 065 * accurate, position should have been obtained. The One-At-A-Time technique is 066 * continued for an additional 200 iterations with the receivers in reverse 067 * order ending with the closest receiver. Weights are applied assuming that 068 * close measurements are more accurate than distant ones. The weights fade out 069 * during the iterations so that at the end the adjustments are very small. This 070 * fade-out fixes a problem with the One-At-A-Time technique in that it gives 071 * undue weight to the last receiver. The result at this point should be quite 072 * accurate, at least for the measurements that are still in play. At this point 073 * the All-Together procedure is run to compute the variance and eliminate an 074 * outlier. Unlike version 2.1, the receiver with the largest residual error is 075 * always discarded--if it is actually OK, it will be put back in Stage 5. The 076 * entire stage-2 procedure is repeated until the number of receivers has been 077 * reduced to 6 or until the iteration count reaches 2000, then moves on to 078 * Stage 3. 079 * <p> 080 * Stages 3 and 5 are new to version 2.2. Stage 3 runs only with 6 and 5 081 * receivers. It solves for all subsets with one receiver removed by running the 082 * One-At-A-Time iteration 250 times, with weights and fade-out, and then using 083 * All-Together to check the variances of the residuals. The receiver which was 084 * removed resulting in the subset with the smallest variance is then discarded 085 * as possibly being errored. Unlike the outlier rejection of Stage 2, which 086 * often discards the wrong receiver, this procedure usually gets it right. But 087 * it is too computationally intensive to use with a large number of receivers. 088 * <p> 089 * Stage 4 runs only with 4 receivers and rejects an "outlier" using the same 090 * procedure as Stage 2. However, it is only run if the variance is considerably 091 * more than just marginally large, i.e. not if there are only small random 092 * measurement errors. With one error in 4 receivers it is theoretically 093 * impossible to determine which one. But, what the heck, there's no harm in 094 * trying. And if it should happen to succeed in ousting the errored receiver 095 * and there are also previously-rejected good receivers that Stage 5 can put 096 * back, then the measurement is salvaged and the correct position is reported 097 * with no error code. 098 * <p> 099 * At this point we may be down to four receivers only (or perhaps three), but 100 * they should be all good receivers. Stage 5 runs the One-At-A-Time iteration 101 * 300 times, with weights and fade-out, and then the All-Together procedure to 102 * get a good solution with only the remaining receivers. Then all of the 103 * rejected receivers are checked to see if any of them agree with this solution 104 * and if so they are put back into the list of good receivers. The reason for 105 * doing this is that the outlier rejection often rejects the wrong receiver. 106 * Since this means we need to do this put-back anyway, Stages 2 to 4 are 107 * arranged to keep on rejecting receivers regardless of whether it does any 108 * good. But Stages 2 to 4 do check the variance and skip to this stage if it 109 * indicates that there are no more significant errors. 110 * <p> 111 * Stage 6 is similar to Stage 3 of version 2.1 except that it runs the 112 * One-At-A-Time iteration in addition to the All-Together iteration, both using 113 * weights according to distance, with the intent to produce a more refined 114 * result. First it runs the One-At-A-Time iteration 250 more times, with 115 * weights and fade-out, to make sure the solution is close since the 116 * All-Together iterations sometimes converge rather slowly. Unlike version 2.1, 117 * it does not attempt to discard outliers. The iterations continue until the 118 * All-Together procedure can do no more or until the total iteration count 119 * reaches 5000. (Note that a single instance of All-Together increments the 120 * iteration count by the number of receivers currently in use.) 121 * <p> 122 * Like version 2.1, this version does not always, or even usually, run through 123 * all the iterations, but instead cuts them short and also cuts out stages when 124 * the solution has converged. The total number of iterations is between 342 125 * (with only 3 receivers) and 5000. The estimated execution time ranges from 126 * 0.13 to ~2.0 milliseconds (1.0 GHz Pentium III). 127 * <p> 128 * Input/output is the same as for previous versions and is described in the 129 * e-mail with version 1.0 sent on 11/17/06. 130 * <p> 131 * Return values are the same as with version 2.1. These are as follows. 132 * <pre> 133 * {@literal >= 4} OK. Value = number of receivers used. 134 * 3 Probably OK. 3 receivers used. 135 * 1,2 Questionable. Maybe OK, maybe not. 136 * 0 No solution. Don't even think about using it. (Position outside the known universe.) 137 * -1,-2 Not used. 138 * {@literal <=} -3 Variance of residuals too big. Probably no good. Value = minus number of receivers used. 139 * (Perhaps close if input is noisy or receiver locations are sloppy.) 140 * </pre> Variance too big means RMS residuals {@literal >} 30 microseconds, 141 * i.e. about 0.4 inch or 1.0 cm. This is about as small a threshold as I dare 142 * use lest random errors occasionally make good measurements appear bad. 143 * <p> 144 * The restrictions on the configuration of transmitters and receivers, 145 * necessary to prevent the program from reporting a spurious position, are the 146 * same as those for version 1.1. These are described in the e-mail with that 147 * version sent on 12/9/06. 148 * 149 * @author Robert Ashenfelter Copyright (C) 2008 150 * @author Bob Jacobsen Copyright (C) 2008 151 */ 152public class Ash2_2Algorithm extends AbstractCalculator { 153 154 @SuppressFBWarnings(value = "ST_WRITE_TO_STATIC_FROM_INSTANCE_METHOD") 155 public Ash2_2Algorithm(Point3d[] sensors, double vsound, int offset) { 156 this(sensors, vsound); 157 Ash2_2Algorithm.offset = offset; 158 } 159 160 public Ash2_2Algorithm(Point3d[] sensors, double vsound) { 161 this.sensors = Arrays.copyOf(sensors, sensors.length); 162 this.Vs = vsound; 163 164 // load the algorithm variables 165 //Point3d origin = new Point3d(); // defaults to 0,0,0 166 } 167 168 public Ash2_2Algorithm(Point3d sensor1, Point3d sensor2, Point3d sensor3, double vsound) { 169 this(new Point3d[]{sensor1, sensor2, sensor3}, vsound); 170 } 171 172 public Ash2_2Algorithm(Point3d sensor1, Point3d sensor2, Point3d sensor3, Point3d sensor4, double vsound) { 173 this(new Point3d[]{sensor1, sensor2, sensor3, sensor4}, vsound); 174 } 175 176 double Vs; 177 double Xt = 0.0; 178 double Yt = 0.0; 179 double Zt = 0.0; 180 181 @Override 182 public Measurement convert(Reading r) { 183 184 if (log.isDebugEnabled()) { 185 log.debug("Reading: {}", r); 186 log.debug("Sensors: {}", sensors.length); 187 if (sensors.length >= 1 && sensors[0] != null) { 188 log.debug("Sensor[0]: {},{},{}", sensors[0].x, sensors[0].y, sensors[0].z); 189 } 190 if (sensors.length >= 2 && sensors[1] != null) { 191 log.debug("Sensor[1]: {},{},{}", sensors[1].x, sensors[1].y, sensors[1].z); 192 } 193 } 194 195 prep(r); 196 197 RetVal result = RPSpos(nr, Tr, Xr, Yr, Zr, Vs, Xt, Yt, Zt); 198 Xt = result.x; 199 Yt = result.y; 200 Zt = result.z; 201 Vs = result.vs; 202 203 log.debug("x = {} y = {} z0 = {} code = {}", Xt, Yt, Zt, result.code); 204 return new Measurement(r, Xt, Yt, Zt, Vs, result.code, "Ash2_2Algorithm"); 205 } 206 207 /** 208 * Seed the conversion using an estimated position 209 */ 210 @Override 211 public Measurement convert(Reading r, Point3d guess) { 212 this.Xt = guess.x; 213 this.Yt = guess.y; 214 this.Zt = guess.z; 215 216 return convert(r); 217 } 218 219 /** 220 * Seed the conversion using a last measurement 221 */ 222 @Override 223 public Measurement convert(Reading r, Measurement last) { 224 if (last != null) { 225 this.Xt = last.getX(); 226 this.Yt = last.getY(); 227 this.Zt = last.getZ(); 228 } 229 230 // if the last measurement failed, set back to origin 231 if (this.Xt > 9.E99) { 232 this.Xt = 0; 233 } 234 if (this.Yt > 9.E99) { 235 this.Yt = 0; 236 } 237 if (this.Zt > 9.E99) { 238 this.Zt = 0; 239 } 240 241 return convert(r); 242 } 243 244 static int offset = 0; // Offset (usec), add to delay 245 246 @SuppressFBWarnings(value = "MS_SHOULD_BE_FINAL") // for script access 247 static public int TMAX = 35000; // Max. allowable delay (usec) 248 @SuppressFBWarnings(value = "MS_SHOULD_BE_FINAL") // for script access 249 static public int TMIN = 150; // Min. allowable delay (usec) 250 @SuppressFBWarnings(value = "MS_SHOULD_BE_FINAL") // for script access 251 static public int SMAX = 30; // Max. OK std. dev. (usec) 252 @SuppressFBWarnings(value = "MS_SHOULD_BE_FINAL") // for script access 253 static public int NMAX = 50; // Max. no. of receivers used 254 @SuppressFBWarnings(value = "MS_SHOULD_BE_FINAL") // for script access 255 static public int NERR = 6; // No. of rcvrs w/error reject 256 257 // Compute RPS Position using 258 RetVal RPSpos(int nr, double Tr[], double Xr[], double Yr[], double Zr[],// many 259 double Vs, double Xt, double Yt, double Zt) {// receivers 260 261 int i = 0, j = 0, jmax = 0, k = 0, l = 0, ns, nss, nxx, nox = 0, S, cmax; 262 int[] ce = new int[NMAX]; 263 double Rq; 264 double[] Rs = new double[NMAX], Xs = new double[NMAX], Ys = new double[NMAX], Zs = new double[NMAX]; 265 double x, y, z, xo = 0., yo = 0., zo = 0., Rmax, Ww, Xw, Yw, Zw, w, q; 266 double err, emax, var = 0, vmax, vmin, vold; 267 double[] vex = new double[NERR]; 268 269 vmax = SMAX * SMAX * Vs * Vs; 270 vmin = 1.0 * Vs * Vs; 271 ns = 0; 272 Rmax = Vs * TMAX; 273 Rs[NMAX - 1] = TMAX;// Sort receivers by delay 274 for (i = 0; i < nr; i++) { 275 if (Tr[i] == 0.0) { 276 continue;// Discard failures 277 } 278 Rq = Vs * (Tr[i] + offset);// Compute range from delay 279 if ((Rq >= Rmax) || (Rq < Vs * TMIN)) { 280 continue;// Discard too long or short 281 } 282 if (ns == 0) { 283 Rs[0] = Rq; 284 Xs[0] = Xr[i]; 285 Ys[0] = Yr[i]; 286 Zs[0] = Zr[i]; 287 ns = 1; 288 }// 1st entry 289 else { 290 j = ((ns == NMAX) ? (ns - 1) : (ns++));// Keep no more than NMAX 291 for (;; j--) {// Bubble sort 292 if ((j > 0) && (Rq < Rs[j - 1])) { 293 Rs[j] = Rs[j - 1]; 294 Xs[j] = Xs[j - 1];// Move old entries 295 Ys[j] = Ys[j - 1]; 296 Zs[j] = Zs[j - 1]; 297 } else { 298 if ((j < NMAX - 1) || (Rq < Rs[j])) {// Insert new entry 299 Rs[j] = Rq; 300 Xs[j] = Xr[i]; 301 Ys[j] = Yr[i]; 302 Zs[j] = Zr[i]; 303 } 304 break; 305 } 306 } 307 } 308 } 309 310 for (i = 0; i < ns; i++) { 311 ce[i] = 0;// Reject gross errors 312 } 313 for (i = 0; i < ns - 1; i++) { 314 for (j = i + 1; j < ns; j++) { 315 q = Math.sqrt((Xs[i] - Xs[j]) * (Xs[i] - Xs[j]) 316 + (Ys[i] - Ys[j]) * (Ys[i] - Ys[j]) + (Zs[i] - Zs[j]) * (Zs[i] - Zs[j])); 317 if ((Rs[i] + Rs[j] < q) || (Rs[i] - Rs[j] > q) || (Rs[j] - Rs[i] > q)) { 318 ++ce[i]; 319 ++ce[j]; 320 } 321 } 322 }// Count them 323 cmax = 1; 324 nxx = 0; 325 while (cmax != 0) {// Repetitively discard 326 cmax = 0;// worst offender 327 for (i = 0; i < ns; i++) { 328 if (ce[i] >= cmax) { 329 if (ce[i] > 0) { 330 nxx = ((ce[i] == cmax) ? nxx + 1 : 1); 331 } 332 cmax = ce[i]; 333 j = i; 334 } 335 }// Find it 336 if (cmax > 0) { 337 for (i = 0; i < ns; i++) {// Adjust remaining counts 338 if (i == j) { 339 continue; 340 } 341 q = Math.sqrt((Xs[i] - Xs[j]) * (Xs[i] - Xs[j]) 342 + (Ys[i] - Ys[j]) * (Ys[i] - Ys[j]) + (Zs[i] - Zs[j]) * (Zs[i] - Zs[j])); 343 if ((Rs[i] + Rs[j] < q) || (Rs[i] - Rs[j] > q) || (Rs[j] - Rs[i] > q)) { 344 --ce[i]; 345 } 346 }// Adjustment 347 for (i = j; i < ns - 1; i++) {// Discard gross error 348 Rs[i] = Rs[i + 1]; 349 Xs[i] = Xs[i + 1]; 350 Ys[i] = Ys[i + 1]; 351 Zs[i] = Zs[i + 1];// Move old entries 352 ce[i] = ce[i + 1]; 353 } 354 --ns; 355 } 356 }// One less receiver 357 nss = ns; 358 359 if (ns < 3) {// Failed: 360 Xt = Yt = Zt = 9.9999999e99; 361 return new RetVal(0, Xt, Yt, Zt, Vs); 362 }// Too few usable receivers 363 364 S = i = 0; 365 x = y = 0.0; 366 z = -100000.0;// Iterative solution 367 while (++i < 5000) { 368 //printf("%4d %2d %3d %d%d%d %lf %lf %lf %lg\r\n",i,j,k,S,nss,ns,x,y,z,var/vmax); 369 if (S == 0) {// Stage 0 370 j = k = (i - 1) % ns;// Receivers in order 371 w = 1.0; 372 }// No wgts. No "All-Tog." 373 else if (S == 1) {// Stage 1 374 j = k = ns - 1 - i % ns; 375 w = 1.0; 376 }// Reverse order, no wgts. 377 else {// Stages 2 and up 378 if (--k < 0) {// Receivers reverse order 379 j = k = 0; 380 w = 0.0; 381 } else { 382 j = k % ns; 383 if ((S == 3) && (j == l)) { 384 j = ((--k > 0) ? k % ns : 0); 385 } 386 w = 1.0 - Rs[j] / Rmax; 387 w = w * w;// Weight by distance 388 if (k < 50) { 389 w *= 0.02 * (k + 1); // with fade out 390 } else { 391 w *= 0.005 * (k + 150); 392 } 393 } 394 } 395 396 if (k >= 0) {// One-At-A-Time iteration 397 q = Math.sqrt((Xs[j] - x) * (Xs[j] - x) + (Ys[j] - y) * (Ys[j] - y) + (Zs[j] - z) * (Zs[j] - z)); 398 q = w * (1.0 - Rs[j] / q);// Adjustment factor 399 x += q * (Xs[j] - x);// Position adjustments 400 y += q * (Ys[j] - y); 401 z += q * (Zs[j] - z); 402 } 403 404 if (((S == 1) && (i % (50 + ns) == 51)) || ((S >= 2) && (k <= 0))) { 405 vold = var; 406 Ww = Xw = Yw = Zw = var = emax = 0.0;// All-Together iteration 407 for (j = 0; j < ns; j++) {// For all receivers 408 if ((S == 3) && (j == l)) { 409 continue; 410 } 411 q = Math.sqrt((Xs[j] - x) * (Xs[j] - x) + (Ys[j] - y) * (Ys[j] - y) + (Zs[j] - z) * (Zs[j] - z)); 412 err = q - Rs[j]; 413 err = err * err;// Residual error 414 q = 1.0 - Rs[j] / q;// Adjustment factor 415 if (S >= 2) { 416 w = 1.0 - Rs[j] / Rmax; 417 w = w * w; 418 }// Weight by distance 419 else { 420 w = 1.0; 421 } 422 Xw += w * (x + q * (Xs[j] - x));// Accumulate averages 423 Yw += w * (y + q * (Ys[j] - y)); 424 Zw += w * (z + q * (Zs[j] - z)); 425 Ww += w; 426 var += w * err; 427 if (w * err > emax) {// Capture max. outlier 428 emax = w * err; 429 jmax = j; 430 } 431 } 432 x = Xw / Ww; 433 y = Yw / Ww; 434 z = Zw / Ww;// Avg. adjusted position 435 var = var / Ww; 436 i += ns - 1; 437 if (((S == 2) && (ns > NERR) && (var > 3 * vmax)) || ((S == 4) && (ns == 4))) { 438 --ns; 439 nox = 0; 440 q = Rs[jmax]; 441 Rs[jmax] = Rs[ns]; 442 Rs[ns] = q;// Discard outlier entry 443 q = Xs[jmax]; 444 Xs[jmax] = Xs[ns]; 445 Xs[ns] = q; 446 q = Ys[jmax]; 447 Ys[jmax] = Ys[ns]; 448 Ys[ns] = q; 449 q = Zs[jmax]; 450 Zs[jmax] = Zs[ns]; 451 Zs[ns] = q; 452 } else { 453 ++nox;// No discard 454 } // Stage Progression 455 if (S == 1) {// Stage 0,1: Initial sol. 456 if (var < vmax) {// Converged: 457 k = 250; 458 nox = 0; 459 S = 6; 460 }// Skip to Stage 6 461 else if ((var < 3 * vmax) || (i >= 750)) { 462 if (ns <= 4) { 463 k = 300; 464 S = (var > 36 * vmax) ? 4 : 5; 465 } // Skip to Stage 4 or 5 466 else if (ns <= NERR) { 467 l = 0; 468 xo = x; 469 yo = y; 470 zo = z;// Advance to Stage 3 471 k = 250; 472 S = 3; 473 } else { 474 k = 200; 475 S = 2; 476 } 477 } 478 }// Advance to Stage 2 479 else if (S == 2) {// Stage 2: Discard outlier 480 if (var < vmax) {// Converged: 481 k = 300; 482 S = 5; 483 }// Skip to Stage 5 484 else if (ns <= NERR) { 485 l = 0; 486 xo = x; 487 yo = y; 488 zo = z;// Advance to Stage 3 489 k = 250; 490 S = 3; 491 } else if (i >= 2000) {// Too much: 492 ns = NERR; // Truncate list 493 l = 0; 494 xo = x; 495 yo = y; 496 zo = z; // and try Stage 3 497 k = 250; 498 S = 3; 499 } 500 k = 200; 501 }// Do another outlier 502 else if (S == 3) {// Stage 3: 503 if (ns > 4) {// Discard errored entry 504 vex[l] = var; // for ns = NERR -> 5 505 if (++l < ns) { // by evaluating subsets 506 k = 250; 507 x = xo; 508 y = yo; 509 z = zo; 510 } // of ns - 1 511 else { // for minimum variance 512 var = vex[j = 0]; 513 for (l = 1; l < ns; l++) {// Find the minimum 514 if (vex[l] < var) { 515 var = vex[j = l]; 516 } 517 } 518 --ns; 519 q = Rs[j]; 520 Rs[j] = Rs[ns]; 521 Rs[ns] = q;// Discard errored entry 522 q = Xs[j]; 523 Xs[j] = Xs[ns]; 524 Xs[ns] = q; 525 q = Ys[j]; 526 Ys[j] = Ys[ns]; 527 Ys[ns] = q; 528 q = Zs[j]; 529 Zs[j] = Zs[ns]; 530 Zs[ns] = q; 531 if (var < vmax) {// Converged: 532 k = 300; 533 S = 5; 534 } // Skip to Stage 5 535 else if (ns <= 4) { 536 k = 300; 537 S = (var > 36 * vmax) ? 4 : 5; 538 } // Skip to Stage 4 or 5 539 else { 540 l = 0; 541 xo = x; 542 yo = y; 543 zo = z; 544 k = 250; 545 } 546 } 547 } 548 }//Do another error 549 else if (S == 4) {// Stage 4: ns = 4 outlier 550 k = 300; 551 S = 5; 552 }// Advance to Stage 5 553 else if (S == 5) {// Stage 5: 554 for (j = ns; j < nss; j++) {// Put back entries 555 q = Math.sqrt((Xs[j] - x) * (Xs[j] - x) + (Ys[j] - y) * (Ys[j] - y) + (Zs[j] - z) * (Zs[j] - z)); 556 if ((Rs[j] - q) * (Rs[j] - q) < 4 * vmax) { // if not errored 557 q = Rs[j]; 558 Rs[j] = Rs[ns]; 559 Rs[ns] = q;// Put back rejected entry 560 q = Xs[j]; 561 Xs[j] = Xs[ns]; 562 Xs[ns] = q; 563 q = Ys[j]; 564 Ys[j] = Ys[ns]; 565 Ys[ns] = q; 566 q = Zs[j]; 567 Zs[j] = Zs[ns]; 568 Zs[ns] = q; 569 ++ns; 570 } 571 } 572 k = 250; 573 nox = 0; 574 S = 6; 575 }// Advance to Stage 6 576 else if (S == 6) {// Stage 6: Final refinement 577 if ((nox >= 1 + 110 / (ns + 5)) && ((var > 0.999 * vold) || (var < vmin))) { 578 break; 579 } 580 }// Advance to done... 581 }// End of All-Together 582 583 if ((S == 0) && (i >= 50)) {// Stage 0: 584 k = j; 585 S = 1; 586 }// Advance to Stage 1 587 }// End of Iteration loop 588 // Done...! 589 Xt = x; 590 Yt = y; 591 Zt = z;// Computed position 592 if ((var > vmax) || ((ns == 3) && (var > vmin))) {// Failed: 593 return new RetVal(-ns, Xt, Yt, Zt, Vs); 594 } // variance too big 595 if ((ns == 3) && (nxx > 1)) {// Questionable: uncertain 596 return new RetVal(1, Xt, Yt, Zt, Vs); 597 } // gross rejection 598 if (nss >= (3 * ns - 5)) {// Questionable: 599 return new RetVal(2, Xt, Yt, Zt, Vs); 600 } // too many rejections 601 return new RetVal(ns, Xt, Yt, Zt, Vs);// Success! (probably...) 602 }// End of RPSpos() 603 604// ---------------------------------------------------- 605 /** 606 * Internal class to handle return value. 607 * 608 * More of a struct, really 609 */ 610 static class RetVal { 611 612 RetVal(int code, double x, double y, double z, double vs) { 613 this.code = code; 614 this.x = x; 615 this.y = y; 616 this.z = z; 617 this.vs = vs; 618 } 619 int code; 620 @SuppressFBWarnings(value = "UUF_UNUSED_FIELD") 621 double x, y, z, t, vs; 622 } 623 624 private final static Logger log = LoggerFactory.getLogger(Ash2_2Algorithm.class); 625 626}