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 1st 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 * @author Bob Jacobsen Copyright (C) 2006 021 */ 022public class Ash1_0Algorithm implements Calculator { 023 024 public Ash1_0Algorithm(Point3d[] sensors, double vsound) { 025 this.sensors = Arrays.copyOf(sensors, sensors.length); 026 this.Vs = vsound; 027 028 // load the algorithm variables 029 //Point3d origin = new Point3d(); // defaults to 0,0,0 030 } 031 032 public Ash1_0Algorithm(Point3d sensor1, Point3d sensor2, Point3d sensor3, double vsound) { 033 this(new Point3d[]{sensor1, sensor2, sensor3}, vsound); 034 } 035 036 public Ash1_0Algorithm(Point3d sensor1, Point3d sensor2, Point3d sensor3, Point3d sensor4, double vsound) { 037 this(new Point3d[]{sensor1, sensor2, sensor3, sensor4}, vsound); 038 } 039 040 double Vs; 041 double Xt = 0.0; 042 double Yt = 0.0; 043 double Zt = 0.0; 044 045 @Override 046 public Measurement convert(Reading r) { 047 048 int nr = r.getNValues(); 049 if (nr != sensors.length) { 050 log.error("Mismatch: {} readings, {} receivers", nr, sensors.length); 051 } 052 nr = Math.min(nr, sensors.length); // accept the shortest 053 054 double[] Tr = new double[nr]; 055 double[] Xr = new double[nr]; 056 double[] Yr = new double[nr]; 057 double[] Zr = new double[nr]; 058 for (int i = 0; i < nr; i++) { 059 Tr[i] = r.getValue(i); 060 Xr[i] = sensors[i].x; 061 Yr[i] = sensors[i].y; 062 Zr[i] = sensors[i].z; 063 } 064 065 RetVal result = RPSpos(nr, Tr, Xr, Yr, Zr, Vs, Xt, Yt, Zt); 066 Xt = result.x; 067 Yt = result.y; 068 Zt = result.z; 069 Vs = result.vs; 070 071 log.debug("x = {} y = {} z0 = {} code = {}", Xt, Yt, Zt, result.code); 072 return new Measurement(r, Xt, Yt, Zt, Vs, result.code, "Ash1_0Algorithm"); 073 } 074 075 /** 076 * Seed the conversion using an estimated position 077 */ 078 @Override 079 public Measurement convert(Reading r, Point3d guess) { 080 this.Xt = guess.x; 081 this.Yt = guess.y; 082 this.Zt = guess.z; 083 084 return convert(r); 085 } 086 087 /** 088 * Seed the conversion using a last measurement 089 */ 090 @Override 091 public Measurement convert(Reading r, Measurement last) { 092 if (last != null) { 093 this.Xt = last.getX(); 094 this.Yt = last.getY(); 095 this.Zt = last.getZ(); 096 } 097 098 // if the last measurement failed, set back to origin 099 if (this.Xt > 9.E99) { 100 this.Xt = 0; 101 } 102 if (this.Yt > 9.E99) { 103 this.Yt = 0; 104 } 105 if (this.Zt > 9.E99) { 106 this.Zt = 0; 107 } 108 109 return convert(r); 110 } 111 112 // Sensor position objects 113 Point3d sensors[]; 114 115 /** 116 * The following is the original algorithm, as provided by Ash as a C 117 * routine 118 */ 119// RPS POSITION SOLVER Version 1.0 by R. C. Ashenfelter 11-17-06 120 121 /* 122 * This algorithm was provided by Robert Ashenfelter 123 * who provides no guarantee as to its usability, 124 * correctness nor intellectual property status. 125 * Use it at your own risk. 126 */ 127 static int OFFSET = 0; // Offset (usec), add to delay 128 static int TMAX = 35000; // Max. allowable delay (usec) 129 static final int NMAX = 15; // Max. no. of receivers used 130 131 double x, y, z, x0, y0, z0, x1, y1, z1, x2, y2, z2, Rmax; 132 double xi, yi, zi, ri, xj, yj, zj, rj, xk, yk, zk, rk; 133 134 // Compute RPS Position using 135 @SuppressFBWarnings(value = "IP_PARAMETER_IS_DEAD_BUT_OVERWRITTEN") // it's secretly FORTRAN.. 136 RetVal RPSpos(int nr, double Tr[], double Xr[], double Yr[], double Zr[],// many 137 double Vs, double Xt, double Yt, double Zt) {// receivers 138 139 int i, j, k, ns; 140 double Rq; 141 double Rs[] = new double[NMAX]; 142 double Xs[] = new double[NMAX]; 143 double Ys[] = new double[NMAX]; 144 double Zs[] = new double[NMAX]; 145 double d, da, db, d11, d12, d21, d22; 146 double x1a = 0, y1a = 0, z1a = 0, x1b = 0, y1b = 0, z1b = 0; 147 double x2a = 0, y2a = 0, z2a = 0, x2b = 0, y2b = 0, z2b = 0; 148 double Ww, Xw, Yw, Zw, w; 149 150 ns = 0; 151 Rs[NMAX - 1] = TMAX; 152 Rmax = Vs * TMAX;// Sort receivers by delay 153 154 for (i = 0; i < nr; i++) { 155 if (Tr[i] == 0) { 156 continue;// Discard failures 157 } 158 Rq = Vs * (Tr[i] + OFFSET);// Compute range from delay 159 if (Rq >= Rmax) { 160 continue;// Discard if too long 161 } 162 if (ns == 0) { 163 Rs[0] = Rq; 164 Xs[0] = Xr[i]; 165 Ys[0] = Yr[i]; 166 Zs[0] = Zr[i]; 167 ns = 1; 168 }// 1st entry 169 else { 170 j = ((ns == NMAX) ? (ns - 1) : (ns++));// Keep no more than NMAX 171 for (;; j--) {// Bubble sort 172 if ((j > 0) && (Rq < Rs[j - 1])) { 173 Rs[j] = Rs[j - 1]; 174 Xs[j] = Xs[j - 1];// Move old entries 175 Ys[j] = Ys[j - 1]; 176 Zs[j] = Zs[j - 1]; 177 } else { 178 if ((j < NMAX - 1) || (Rq < Rs[j])) {// Insert new entry 179 Rs[j] = Rq; 180 Xs[j] = Xr[i]; 181 Ys[j] = Yr[i]; 182 Zs[j] = Zr[i]; 183 } 184 break; 185 } 186 } 187 } 188 } 189 190 if (ns < 3) {// Failed: 191 Xt = Yt = Zt = 9.9999999e99; 192 return new RetVal(1, Xt, Yt, Zt, Vs); 193 }// Too few usable receivers 194 195 da = db = 0.0;// Initial solution 196 for (i = 0; i < ns; i++) {// to reject spurious sol. 197 j = (i + 1) % ns; 198 k = (i + 2) % ns;// For ns sets 199 xi = Xs[i]; 200 yi = Ys[i]; 201 zi = Zs[i]; 202 ri = Rs[i]; 203 xj = Xs[j]; 204 yj = Ys[j]; 205 zj = Zs[j]; 206 rj = Rs[j]; 207 xk = Xs[k]; 208 yk = Ys[k]; 209 zk = Zs[k]; 210 rk = Rs[k]; 211 if (gps3() == 0) {// Solve for one set 212 x = x1; 213 y = y1; 214 z = z1;// Reject if no solution 215 if (wgt() > 0.0) { 216 x = x2; 217 y = y2; 218 z = z2;// or no weight 219 if (wgt() > 0.0) { 220 d = (x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2) + (z1 - z2) * (z1 - z2); 221 if ((d < da) || (da == 0.0)) {// Set with minimum 222 da = d;// solution distance 223 x1a = x1; 224 y1a = y1; 225 z1a = z1; 226 x2a = x2; 227 y2a = y2; 228 z2a = z2; 229 } 230 if (d >= db) {// Set with maximum 231 db = d;// solution distance 232 x1b = x1; 233 y1b = y1; 234 z1b = z1; 235 x2b = x2; 236 y2b = y2; 237 z2b = z2; 238 } 239 } 240 } 241 } 242 } 243 if ((da == 0.0) && (db == 0.0)) {// No solution found 244 x = y = z = 0.0; 245 } else { 246 d11 = (x1a - x1b) * (x1a - x1b) + (y1a - y1b) * (y1a - y1b) + (z1a - z1b) * (z1a - z1b); 247 d12 = (x1a - x2b) * (x1a - x2b) + (y1a - y2b) * (y1a - y2b) + (z1a - z2b) * (z1a - z2b); 248 d21 = (x2a - x1b) * (x2a - x1b) + (y2a - y1b) * (y2a - y1b) + (z2a - z1b) * (z2a - z1b); 249 d22 = (x2a - x2b) * (x2a - x2b) + (y2a - y2b) * (y2a - y2b) + (z2a - z2b) * (z2a - z2b); 250 if ((d11 < d12) && (d11 < d21) && (d11 < d22)) {// Choose solution 251 x = (x1a + x1b) / 2; 252 y = (y1a + y1b) / 2; 253 z = (z1a + z1b) / 2; 254 } else if ((d12 < d21) && (d12 < d22)) {// which is 255 x = (x1a + x2b) / 2; 256 y = (y1a + y2b) / 2; 257 z = (z1a + z2b) / 2; 258 } else if (d21 < d22) {// closest between 259 x = (x2a + x1b) / 2; 260 y = (y2a + y1b) / 2; 261 z = (z2a + z1b) / 2; 262 } else {// max. and min. sets 263 x = (x2a + x2b) / 2; 264 y = (y2a + y2b) / 2; 265 z = (z2a + z2b) / 2; 266 } 267 } 268 269 Ww = Xw = Yw = Zw = 0.0;// Final solution 270 for (i = 0; i < ns - 2; i++) {// Weighted average 271 xi = Xs[i]; 272 yi = Ys[i]; 273 zi = Zs[i]; 274 ri = Rs[i];// of all sets 275 for (j = i + 1; j < ns - 1; j++) { 276 xj = Xs[j]; 277 yj = Ys[j]; 278 zj = Zs[j]; 279 rj = Rs[j]; 280 for (k = j + 1; k < ns; k++) { 281 xk = Xs[k]; 282 yk = Ys[k]; 283 zk = Zs[k]; 284 rk = Rs[k]; 285 if (gps3() == 0) {// Solve for one set 286 if (Ww == 0.0) { 287 x = x0; 288 y = y0; 289 z = z0; 290 }// Initial position 291 if ((w = wgt()) > 0.0) { 292 Ww += w; 293 Xw += w * x0; 294 Yw += w * y0; 295 Zw += w * z0;// Add to averages 296 x = Xw / Ww; 297 y = Yw / Ww; 298 z = Zw / Ww; 299 } 300 } 301 } 302 } 303 }// Latest position 304 305 if (Ww > 0.0) { 306 Xt = x; 307 Yt = y; 308 Zt = z;// Computed position 309 return new RetVal(0, Xt, Yt, Zt, Vs); 310 }// Success 311 else { 312 Xt = Yt = Zt = 9.9999999e99; 313 return new RetVal(2, Xt, Yt, Zt, Vs); 314 }// Failed: No solution 315 }// End of RPSpos() 316 317 double wgt() {// Weighting Function 318 double w; 319 320 w = (1 - ri / Rmax) * (1 - rj / Rmax) * (1 - rk / Rmax); // Ranges 321 w *= 1.0 - Math.pow(((x - xi) * (x - xj) + (y - yi) * (y - yj) + (z - zi) * (z - zj)) / ri / rj, 2); // Angles 322 w *= 1.0 - Math.pow(((x - xi) * (x - xk) + (y - yi) * (y - yk) + (z - zi) * (z - zk)) / ri / rk, 2); 323 w *= 1.0 - Math.pow(((x - xj) * (x - xk) + (y - yj) * (y - yk) + (z - zj) * (z - zk)) / rj / rk, 2); 324 w *= 0.05 + Math.abs((zi + zj + zk - 3 * z) / (ri + rj + rk)); // Verticality 325 w *= (((yk - yi) * (zj - zi) - (yj - yi) * (zk - zi)) * (x - xi) 326 + ((zk - zi) * (xj - xi) - (zj - zi) * (xk - xi)) * (y - yi) 327 + ((xk - xi) * (yj - yi) - (xj - xi) * (yk - yi)) * (z - zi)) / (ri * rj * rk); // Volume 328 w = Math.abs(w); 329 if ((w > 0.5) || (w < .0000005)) { 330 w = 0.0; 331 } 332 return (w); 333 } 334 335 int gps3() {// GPS Position Solver 336 double xik, yik, zik;// Inputs (global variables) 337 double xjk, yjk, zjk; 338 double Ax, Ay, Az, Bx, By, Bz, Dx, Dy, Dz;// sat. position, range: 339 double Ci, Cj, Cx, Cy, Cz; // xi, yi, zi, ri 340 double e1, e2; // xj, yj, zj, rj 341 // xk, yk, zk, rk 342 343 xik = xi - xk; 344 yik = yi - yk; 345 zik = zi - zk;// Solve with absolute ranges 346 xjk = xj - xk; 347 yjk = yj - yk; 348 zjk = zj - zk; 349 Ci = (xi * xi - xk * xk + yi * yi - yk * yk + zi * zi - zk * zk - ri * ri + rk * rk) / 2; 350 Cj = (xj * xj - xk * xk + yj * yj - yk * yk + zj * zj - zk * zk - rj * rj + rk * rk) / 2; 351 Dz = xik * yjk - xjk * yik; 352 Dy = zik * xjk - zjk * xik; 353 Dx = yik * zjk - yjk * zik; 354 355 if ((Math.abs(Dx) > Math.abs(Dy)) && (Math.abs(Dx) > Math.abs(Dz))) {// Favoring x-axis 356 Ay = (zik * xjk - zjk * xik) / Dx; 357 By = (zjk * Ci - zik * Cj) / Dx; 358 Az = (yjk * xik - yik * xjk) / Dx; 359 Bz = (yik * Cj - yjk * Ci) / Dx; 360 Ax = Ay * Ay + Az * Az + 1.0; 361 Bx = (Ay * (yk - By) + Az * (zk - Bz) + xk) / Ax; 362 Cx = Bx * Bx - (By * By + Bz * Bz - 2 * yk * By - 2 * zk * Bz + yk * yk + zk * zk + xk * xk - rk * rk) / Ax; 363 if (Cx < 0.0) { 364 x0 = y0 = z0 = 9.9999999e99;// If no solution, 365 return 1; // make it far, far away. 366 } 367 x1 = Bx + Math.sqrt(Cx); 368 y1 = Ay * x1 + By; 369 z1 = Az * x1 + Bz; 370 x2 = 2 * Bx - x1; 371 y2 = Ay * x2 + By; 372 z2 = Az * x2 + Bz; 373 } else if (Math.abs(Dy) > Math.abs(Dz)) {// Favoring y-axis 374 Az = (xik * yjk - xjk * yik) / Dy; 375 Bz = (xjk * Ci - xik * Cj) / Dy; 376 Ax = (zjk * yik - zik * yjk) / Dy; 377 Bx = (zik * Cj - zjk * Ci) / Dy; 378 Ay = Az * Az + Ax * Ax + 1.0; 379 By = (Az * (zk - Bz) + Ax * (xk - Bx) + yk) / Ay; 380 Cy = By * By - (Bz * Bz + Bx * Bx - 2 * zk * Bz - 2 * xk * Bx + zk * zk + xk * xk + yk * yk - rk * rk) / Ay; 381 if (Cy < 0.0) { 382 x0 = y0 = z0 = 9.9999999e99;// If no solution, 383 return 1; // make it far, far away. 384 } 385 y1 = By + Math.sqrt(Cy); 386 z1 = Az * y1 + Bz; 387 x1 = Ax * y1 + Bx; 388 y2 = 2 * By - y1; 389 z2 = Az * y2 + Bz; 390 x2 = Ax * y2 + Bx; 391 } else {// Favoring z-axis 392 if (Dz == 0.0) { 393 x0 = y0 = z0 = 9.9999999e99;// If no solution, 394 return 1; // make it far, far away. 395 } 396 Ax = (yik * zjk - yjk * zik) / Dz; 397 Bx = (yjk * Ci - yik * Cj) / Dz; 398 Ay = (xjk * zik - xik * zjk) / Dz; 399 By = (xik * Cj - xjk * Ci) / Dz; 400 Az = Ax * Ax + Ay * Ay + 1.0; 401 Bz = (Ax * (xk - Bx) + Ay * (yk - By) + zk) / Az; 402 Cz = Bz * Bz - (Bx * Bx + By * By - 2 * xk * Bx - 2 * yk * By + xk * xk + yk * yk + zk * zk - rk * rk) / Az; 403 if (Cz < 0.0) { 404 x0 = y0 = z0 = 9.9999999e99;// If no solution, 405 return 1; // make it far, far away. 406 } 407 z1 = Bz + Math.sqrt(Cz); 408 x1 = Ax * z1 + Bx; 409 y1 = Ay * z1 + By; 410 z2 = 2 * Bz - z1; 411 x2 = Ax * z2 + Bx; 412 y2 = Ay * z2 + By; 413 } 414 415 e1 = (x - x1) * (x - x1) + (y - y1) * (y - y1) + (z - z1) * (z - z1);// Pick solution closest 416 e2 = (x - x2) * (x - x2) + (y - y2) * (y - y2) + (z - z2) * (z - z2);// to x, y, z 417 if (e1 <= e2) {// (also global inputs) 418 x0 = x1; 419 y0 = y1; 420 z0 = z1; 421 }//Solution (global variables) 422 else { 423 x0 = x2; 424 y0 = y2; 425 z0 = z2; 426 }// GPS Position = x0, y0, z0 427 return 0; 428 } 429 private final static Logger log = LoggerFactory.getLogger(Ash1_0Algorithm.class); 430 431 /** 432 * Internal class to handle return value. 433 * 434 * More of a struct, really 435 */ 436 @SuppressFBWarnings(value = "UUF_UNUSED_FIELD") // t not formally needed 437 static class RetVal { 438 439 RetVal(int code, double x, double y, double z, double vs) { 440 this.code = code; 441 this.x = x; 442 this.y = y; 443 this.z = z; 444 this.vs = vs; 445 } 446 int code; 447 double x, y, z, t, vs; 448 } 449 450} 451 452