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 version 1.1 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 * 021 * The following is a summary of this version from Robert Ashenfelter: 022 * <p> 023 * When the receivers are in a plane or nearly so there is a spurious solution 024 * on the other side of the plane from the correct solution and in certain 025 * situations the version 1.0 program may choose the wrong solution. 026 * <p> 027 * It turns out that those situations are when the receiver configuration is not 028 * sufficiently non-planar for the size of the measurement errors. The greater 029 * the errors, the greater the non-planarity must be to avoid he bug. 030 * <p> 031 * I had hoped to be able to devise some measure of the non-planarity of the 032 * receiver configuration and to set some threshold below which the program 033 * would switch to a different algorithm but this didn't seem to be working out 034 * very well. After trying several things, I have chosen to use an iterative 035 * procedure to determine an approximate solution. 036 * <p> 037 * Here is a description of the new program. 038 * <p> 039 * As before the first thing it does is sort the receivers in order of 040 * increasing time delay, discarding those that failed or are too far or too 041 * near, and using the closest ones. There is a maximum that are used, still set 042 * at 15. 043 * <p> 044 * Next it computes a preliminary transmitter position which is used to 045 * discriminate against spurious solutions and to compute weights. This is the 046 * part of the program that has been changed to fix the bug. The new algorithm 047 * looks at one receiver at a time and moves the estimated position directly 048 * toward or away from it such that the distance is equal to the measured value. 049 * After going through the receivers once in order, it then chooses them at 050 * random until it has iterated some fixed number of times. This is set at 1000 051 * although the procedure usually converges in 20-50 iterations; for occasional 052 * positions the convergence is much slower. The random order is used because 053 * the procedure was occasionally observed to get stuck in a loop when using a 054 * repetitive fixed order. Rather than start with the origin as the initial 055 * position, it now starts from a position far, far below. This removes the 056 * restriction that the origin must be below the receivers. 057 * <p> 058 * Finally, as before, the transmitter position is computed as a weighted 059 * average of the GPS solutions for all possible sets of three receivers. (For 060 * 15 receivers, that's 455 solutions.) The weights are the same as before. 061 * Unless one of them chooses a spurious solution, both versions of the program 062 * produce the same computed position. 063 * <p> 064 * Restrictions: 065 * <ol> 066 * <li>The origin can be anywhere, but the z-axis must be vertical with positive 067 * z upward. 068 * 069 * <li>In general, the transmitter should be below some or all of the receivers. 070 * How much below depends on the receiver configuration. 071 * 072 * <li>If the receivers are in a plane, or nearly so, the transmitter must 073 * absolutely be below the plane. As it approaches the plane (such that the 074 * lines-of-sight to the receivers make shallow angles with respect to the 075 * plane), the accuracy degrades and ultimately the program may report failure. 076 * If above the plane, the program reports incorrect positions. 077 * 078 * <li>If the receivers are not in a plane, it may be possible to move the 079 * transmitter up among them. In general it should remain inside or below the 080 * volume of space contained by the receivers. However if the configuration is 081 * sufficiently non-planar the transmitter can go farther. But the limits are 082 * uncertain and there is no warning as it approaches a limit; the reported 083 * position suddenly jumps to somewhere else, or perhaps it jumps back and forth 084 * depending on measurement errors. An extreme example is 8 receivers at the 085 * corners of a cube, which is about as non-planar as it gets. In this case the 086 * transmitter can go outside the cube by several times the width of the cube, 087 * both laterally and vertically, before the program gets into trouble. 088 * </ol> 089 * <p> 090 * I have tested the program with nearly 20 different receiver configurations 091 * having from 3 to 100 receivers. Most were tested at 60 or more transmitter 092 * locations and with infinitesimal, nominal (+/-0.25 inches--Walter's spec.), 093 * and large (+/-2.5 inches) measurement errors. Half of the configurations 094 * consisted of a 10 x 20-foot room with the ceiling 5 feet above the lowest 095 * transmitter positions and the receivers (from 3 to 18) located on the walls 096 * and/or ceiling. Other configurations are Larry Wade's rather-small oval test 097 * track with 4 receivers and a 14-foot square and an 8-foot cube (mentioned 098 * above). Large, 100-receiver configurations include a 25 x 10 x 5-foot space 099 * with receivers randomly located throughout (rather unrealistic) and a 100 x 100 * 10 x 5-foot space with receivers arranged in 4 rows of 25, one row on each 101 * long wall and two rows on the ceiling. Performance (i.e. accuracy of the 102 * measured transmitter position) is excellent throughout this latter space. 103 * <p> 104 * Two other configurations are 20-foot-diameter geodesic domes with receivers 105 * located at the vertices of the triangular faces of the domes, one with 16 106 * receivers and one with 46. Performance is good throughout the interior of 107 * these domes, but surprisingly it is no better with 46 receivers than with 16, 108 * near the perimeter a bit worse. Presumably this is because of the limited 109 * number of closest receivers used by the position program. In order to do 110 * justice to this, or any other configuration with closely-spaced receivers, 111 * the program needs to use data from more than the 15 receivers currently used. 112 * <p> 113 * As a result of all this testing, I feel pretty confident that version 1.1 114 * works reliably if used within the restrictions listed above. But the 115 * disclaimer about "usability and correctness" stays. 116 * <p> 117 * The execution time is increased a little by all those iterations. It now 118 * ranges from 0.5 millisecond with 3 receivers to 1.9 millisecond with 15 or 119 * more receivers (1.0 GHz Pentium III). 120 * 121 * @author Robert Ashenfelter Copyright (C) 2006 122 * @author Bob Jacobsen Copyright (C) 2006 123 */ 124public class Ash1_1Algorithm implements Calculator { 125 126 public Ash1_1Algorithm(Point3d[] sensors, double vsound) { 127 this.sensors = Arrays.copyOf(sensors, sensors.length); 128 this.Vs = vsound; 129 130 // load the algorithm variables 131 //Point3d origin = new Point3d(); // defaults to 0,0,0 132 } 133 134 public Ash1_1Algorithm(Point3d sensor1, Point3d sensor2, Point3d sensor3, double vsound) { 135 this(new Point3d[]{sensor1, sensor2, sensor3}, vsound); 136 } 137 138 public Ash1_1Algorithm(Point3d sensor1, Point3d sensor2, Point3d sensor3, Point3d sensor4, double vsound) { 139 this(new Point3d[]{sensor1, sensor2, sensor3, sensor4}, vsound); 140 } 141 142 double Vs; 143 double Xt = 0.0; 144 double Yt = 0.0; 145 double Zt = 0.0; 146 147 @Override 148 public Measurement convert(Reading r) { 149 150 int nr = r.getNValues(); 151 if (nr != sensors.length) { 152 log.error("Mismatch: {} readings, {} receivers", nr, sensors.length); 153 } 154 nr = Math.min(nr, sensors.length); // accept the shortest 155 156 double[] Tr = new double[nr]; 157 double[] Xr = new double[nr]; 158 double[] Yr = new double[nr]; 159 double[] Zr = new double[nr]; 160 for (int i = 0; i < nr; i++) { 161 Tr[i] = r.getValue(i); 162 Xr[i] = sensors[i].x; 163 Yr[i] = sensors[i].y; 164 Zr[i] = sensors[i].z; 165 } 166 167 RetVal result = RPSpos(nr, Tr, Xr, Yr, Zr, Vs, Xt, Yt, Zt); 168 Xt = result.x; 169 Yt = result.y; 170 Zt = result.z; 171 Vs = result.vs; 172 173 log.debug("x = {} y = {} z0 = {} code = {}", Xt, Yt, Zt, result.code); 174 return new Measurement(r, Xt, Yt, Zt, Vs, result.code, "Ash1_1Algorithm"); 175 } 176 177 /** 178 * Seed the conversion using an estimated position 179 */ 180 @Override 181 public Measurement convert(Reading r, Point3d guess) { 182 this.Xt = guess.x; 183 this.Yt = guess.y; 184 this.Zt = guess.z; 185 186 return convert(r); 187 } 188 189 /** 190 * Seed the conversion using a last measurement 191 */ 192 @Override 193 public Measurement convert(Reading r, Measurement last) { 194 if (last != null) { 195 this.Xt = last.getX(); 196 this.Yt = last.getY(); 197 this.Zt = last.getZ(); 198 } 199 200 // if the last measurement failed, set back to origin 201 if (this.Xt > 9.E99) { 202 this.Xt = 0; 203 } 204 if (this.Yt > 9.E99) { 205 this.Yt = 0; 206 } 207 if (this.Zt > 9.E99) { 208 this.Zt = 0; 209 } 210 211 return convert(r); 212 } 213 214 // Sensor position objects 215 Point3d sensors[]; 216 217 /** 218 * The following is the original algorithm, as provided by Ash as a C 219 * routine 220 */ 221// RPS POSITION SOLVER Version 1.1 by R. C. Ashenfelter 12-02-06 222 223 /* 224 * This algorithm was provided by Robert Ashenfelter 225 * who provides no guarantee as to its usability, 226 * correctness nor intellectual property status. 227 * Use it at your own risk. 228 */ 229 static final int OFFSET = 0; // Offset (usec), add to delay 230 static final int TMAX = 35000; // Max. allowable delay (usec) 231 static final int TMIN = 150; // Min. allowable delay (usec) 232 static final int NMAX = 15; // Max. no. of receivers used 233 234 double x, y, z, x0, y0, z0, Rmax; 235 double xi, yi, zi, ri, xj, yj, zj, rj, xk, yk, zk, rk; 236 237 // Compute RPS Position using 238 @SuppressFBWarnings(value = "IP_PARAMETER_IS_DEAD_BUT_OVERWRITTEN") // it's secretly FORTRAN.. 239 RetVal RPSpos(int nr, double Tr[], double Xr[], double Yr[], double Zr[],// many 240 double Vs, double Xt, double Yt, double Zt) {// receivers 241 242 int i, j, k, ns; 243 double Rq; 244 double[] Rs = new double[NMAX]; 245 double[] Xs = new double[NMAX]; 246 double[] Ys = new double[NMAX]; 247 double[] Zs = new double[NMAX]; 248 double Ww, Xw, Yw, Zw, w; 249 250 k = 0; 251 252 ns = 0; 253 Rs[NMAX - 1] = TMAX; 254 Rmax = Vs * TMAX;// Sort receivers by delay 255 for (i = 0; i < nr; i++) { 256 if (Tr[i] == 0.0) { 257 continue;// Discard failures 258 } 259 Rq = Vs * (Tr[i] + OFFSET);// Compute range from delay 260 if ((Rq >= Rmax) || (Rq < Vs * TMIN)) { 261 continue;// Discard too long or short 262 } 263 if (ns == 0) { 264 Rs[0] = Rq; 265 Xs[0] = Xr[i]; 266 Ys[0] = Yr[i]; 267 Zs[0] = Zr[i]; 268 ns = 1; 269 }// 1st entry 270 else { 271 j = ((ns == NMAX) ? (ns - 1) : (ns++));// Keep no more than NMAX 272 for (;; j--) {// Bubble sort 273 if ((j > 0) && (Rq < Rs[j - 1])) { 274 Rs[j] = Rs[j - 1]; 275 Xs[j] = Xs[j - 1];// Move old entries 276 Ys[j] = Ys[j - 1]; 277 Zs[j] = Zs[j - 1]; 278 } else { 279 if ((j < NMAX - 1) || (Rq < Rs[j])) {// Insert new entry 280 Rs[j] = Rq; 281 Xs[j] = Xr[i]; 282 Ys[j] = Yr[i]; 283 Zs[j] = Zr[i]; 284 } 285 break; 286 } 287 } 288 } 289 } 290 291 if (ns < 3) {// Failed: 292 Xt = Yt = Zt = 9.9999999e99; 293 return new RetVal(1, Xt, Yt, Zt, Vs); 294 }// Too few usable receivers 295 296 x = y = 0.0; 297 z = -100000.0;// Initial solution 298 for (i = 0; i < 1000; i++) {// to reject spurious sol. 299//if (i%10 == 1) printf("\n%4d %8.3lf %8.3lf %8.3lf",i,x,y,z); 300 if (i < ns) { 301 j = i;// and to calc. weights 302 } else { 303 while ((j = (int) Math.floor( 304 (ns) * Math.random() 305 )) 306 == k) { 307 // Iterative solution 308 } 309 } 310 311 k = j; 312 w = Math.sqrt((Xs[j] - x) * (Xs[j] - x) + (Ys[j] - y) * (Ys[j] - y) + (Zs[j] - z) * (Zs[j] - z)); 313 w = Rs[j] / w; 314 x = w * (x - Xs[j]) + Xs[j];// with 315 y = w * (y - Ys[j]) + Ys[j]; 316 z = w * (z - Zs[j]) + Zs[j]; 317 }// 1000 random receivers 318 319 Ww = Xw = Yw = Zw = 0.0;// Final solution 320 for (i = 0; i < ns - 2; i++) {// Weighted average 321 xi = Xs[i]; 322 yi = Ys[i]; 323 zi = Zs[i]; 324 ri = Rs[i];// of all sets 325 for (j = i + 1; j < ns - 1; j++) { 326 xj = Xs[j]; 327 yj = Ys[j]; 328 zj = Zs[j]; 329 rj = Rs[j]; 330 for (k = j + 1; k < ns; k++) { 331 xk = Xs[k]; 332 yk = Ys[k]; 333 zk = Zs[k]; 334 rk = Rs[k]; 335 if (gps3() == 0) {// Solve for each set 336 if ((w = wgt()) > 0.0) {// Add to averages 337 Ww += w; 338 Xw += w * x0; 339 Yw += w * y0; 340 Zw += w * z0; 341 } 342 } 343 } 344 } 345 } 346 347 if (Ww > 0.0) { 348 Xt = Xw / Ww; 349 Yt = Yw / Ww; 350 Zt = Zw / Ww;// Computed position 351 return new RetVal(0, Xt, Yt, Zt, Vs); 352 }// Success 353 else { 354 Xt = Yt = Zt = 9.9999999e99; 355 return new RetVal(2, Xt, Yt, Zt, Vs); 356 }// Failed: No solution 357 }// End of RPSpos() 358 359 double wgt() {// Weighting Function 360 double w; 361 362 w = (1 - ri / Rmax) * (1 - rj / Rmax) * (1 - rk / Rmax); // Ranges 363 w *= 1.0 - Math.pow(((x - xi) * (x - xj) + (y - yi) * (y - yj) + (z - zi) * (z - zj)) / ri / rj, 2.); // Angles 364 w *= 1.0 - Math.pow(((x - xi) * (x - xk) + (y - yi) * (y - yk) + (z - zi) * (z - zk)) / ri / rk, 2.); 365 w *= 1.0 - Math.pow(((x - xj) * (x - xk) + (y - yj) * (y - yk) + (z - zj) * (z - zk)) / rj / rk, 2.); 366 w *= 0.05 + Math.abs((zi + zj + zk - 3 * z) / (ri + rj + rk)); // Verticality 367 w *= (((yk - yi) * (zj - zi) - (yj - yi) * (zk - zi)) * (x - xi) 368 + ((zk - zi) * (xj - xi) - (zj - zi) * (xk - xi)) * (y - yi) 369 + ((xk - xi) * (yj - yi) - (xj - xi) * (yk - yi)) * (z - zi)) / (ri * rj * rk); // Volume 370 w = Math.abs(w); 371 if ((w > 0.5) || (w < .0000005)) { 372 w = 0.0; 373 } 374 return (w); 375 } 376 377 int gps3() {// GPS Position Solver 378 double xik, yik, zik;// Inputs (global variables) 379 double xjk, yjk, zjk;// sat. position, range: 380 double Ax, Ay, Az, Bx, By, Bz, Dx, Dy, Dz; // xi, yi, zi, ri 381 double Ci, Cj, Cx, Cy, Cz; // xj, yj, zj, rj 382 double x1, y1, z1, x2, y2, z2, e1, e2; // xk, yk, zk, rk 383 384 xik = xi - xk; 385 yik = yi - yk; 386 zik = zi - zk;// Solve with absolute ranges 387 xjk = xj - xk; 388 yjk = yj - yk; 389 zjk = zj - zk; 390 Ci = (xi * xi - xk * xk + yi * yi - yk * yk + zi * zi - zk * zk - ri * ri + rk * rk) / 2; 391 Cj = (xj * xj - xk * xk + yj * yj - yk * yk + zj * zj - zk * zk - rj * rj + rk * rk) / 2; 392 Dz = xik * yjk - xjk * yik; 393 Dy = zik * xjk - zjk * xik; 394 Dx = yik * zjk - yjk * zik; 395 396 if ((Math.abs(Dx) > Math.abs(Dy)) && (Math.abs(Dx) > Math.abs(Dz))) {// Favoring x-axis 397 Ay = (zik * xjk - zjk * xik) / Dx; 398 By = (zjk * Ci - zik * Cj) / Dx; 399 Az = (yjk * xik - yik * xjk) / Dx; 400 Bz = (yik * Cj - yjk * Ci) / Dx; 401 Ax = Ay * Ay + Az * Az + 1.0; 402 Bx = (Ay * (yk - By) + Az * (zk - Bz) + xk) / Ax; 403 Cx = Bx * Bx - (By * By + Bz * Bz - 2 * yk * By - 2 * zk * Bz + yk * yk + zk * zk + xk * xk - rk * rk) / Ax; 404 if (Cx < 0.0) { 405 x0 = y0 = z0 = 9.9999999e99;// If no solution, 406 return 1; 407 }// make it far, far away. 408 x1 = Bx + Math.sqrt(Cx); 409 y1 = Ay * x1 + By; 410 z1 = Az * x1 + Bz; 411 x2 = 2 * Bx - x1; 412 y2 = Ay * x2 + By; 413 z2 = Az * x2 + Bz; 414 } else if (Math.abs(Dy) > Math.abs(Dz)) {// Favoring y-axis 415 Az = (xik * yjk - xjk * yik) / Dy; 416 Bz = (xjk * Ci - xik * Cj) / Dy; 417 Ax = (zjk * yik - zik * yjk) / Dy; 418 Bx = (zik * Cj - zjk * Ci) / Dy; 419 Ay = Az * Az + Ax * Ax + 1.0; 420 By = (Az * (zk - Bz) + Ax * (xk - Bx) + yk) / Ay; 421 Cy = By * By - (Bz * Bz + Bx * Bx - 2 * zk * Bz - 2 * xk * Bx + zk * zk + xk * xk + yk * yk - rk * rk) / Ay; 422 if (Cy < 0.0) { 423 x0 = y0 = z0 = 9.9999999e99;// If no solution, 424 return 1; 425 }// make it far, far away. 426 y1 = By + Math.sqrt(Cy); 427 z1 = Az * y1 + Bz; 428 x1 = Ax * y1 + Bx; 429 y2 = 2 * By - y1; 430 z2 = Az * y2 + Bz; 431 x2 = Ax * y2 + Bx; 432 } else {// Favoring z-axis 433 if (Dz == 0.0) { 434 x0 = y0 = z0 = 9.9999999e99;// If no solution, 435 return 1; 436 }// make it far, far away. 437 Ax = (yik * zjk - yjk * zik) / Dz; 438 Bx = (yjk * Ci - yik * Cj) / Dz; 439 Ay = (xjk * zik - xik * zjk) / Dz; 440 By = (xik * Cj - xjk * Ci) / Dz; 441 Az = Ax * Ax + Ay * Ay + 1.0; 442 Bz = (Ax * (xk - Bx) + Ay * (yk - By) + zk) / Az; 443 Cz = Bz * Bz - (Bx * Bx + By * By - 2 * xk * Bx - 2 * yk * By + xk * xk + yk * yk + zk * zk - rk * rk) / Az; 444 if (Cz < 0.0) { 445 x0 = y0 = z0 = 9.9999999e99;// If no solution, 446 return 1; 447 }// make it far, far away. 448 z1 = Bz + Math.sqrt(Cz); 449 x1 = Ax * z1 + Bx; 450 y1 = Ay * z1 + By; 451 z2 = 2 * Bz - z1; 452 x2 = Ax * z2 + Bx; 453 y2 = Ay * z2 + By; 454 } 455 456 e1 = (x - x1) * (x - x1) + (y - y1) * (y - y1) + (z - z1) * (z - z1);// Pick solution closest 457 e2 = (x - x2) * (x - x2) + (y - y2) * (y - y2) + (z - z2) * (z - z2);// to x, y, z 458 if (e1 <= e2) {// (also global inputs) 459 x0 = x1; 460 y0 = y1; 461 z0 = z1; 462 }//Solution (global variables) 463 else { 464 x0 = x2; 465 y0 = y2; 466 z0 = z2; 467 }// GPS Position = x0, y0, z0 468 return 0; 469 } 470 471// ****************** 472 private final static Logger log = LoggerFactory.getLogger(Ash1_1Algorithm.class); 473 474 /** 475 * Internal class to handle return value. 476 * 477 * More of a struct, really 478 */ 479 @SuppressFBWarnings(value = "UUF_UNUSED_FIELD") // t not formally needed 480 static class RetVal { 481 482 RetVal(int code, double x, double y, double z, double vs) { 483 this.code = code; 484 this.x = x; 485 this.y = y; 486 this.z = z; 487 this.vs = vs; 488 } 489 int code; 490 double x, y, z, t, vs; 491 } 492 493}