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 * @author Robert Ashenfelter Copyright (C) 2007 021 * @author Bob Jacobsen Copyright (C) 2007 022 */ 023public class Ash2_1Algorithm extends AbstractCalculator { 024 025 @SuppressFBWarnings(value = "ST_WRITE_TO_STATIC_FROM_INSTANCE_METHOD") 026 public Ash2_1Algorithm(Point3d[] sensors, double vsound, int offset) { 027 this(sensors, vsound); 028 this.offset = offset; 029 } 030 031 public Ash2_1Algorithm(Point3d[] sensors, double vsound) { 032 this.sensors = Arrays.copyOf(sensors, sensors.length); 033 this.Vs = vsound; 034 035 // load the algorithm variables 036 //Point3d origin = new Point3d(); // defaults to 0,0,0 037 } 038 039 public Ash2_1Algorithm(Point3d sensor1, Point3d sensor2, Point3d sensor3, double vsound) { 040 this(new Point3d[]{sensor1, sensor2, sensor3}, vsound); 041 } 042 043 public Ash2_1Algorithm(Point3d sensor1, Point3d sensor2, Point3d sensor3, Point3d sensor4, double vsound) { 044 this(new Point3d[]{sensor1, sensor2, sensor3, sensor4}, vsound); 045 } 046 047 double Vs; 048 double Xt = 0.0; 049 double Yt = 0.0; 050 double Zt = 0.0; 051 052 @Override 053 public Measurement convert(Reading r) { 054 055 if (log.isDebugEnabled()) { 056 log.debug("Reading: {}", r); 057 log.debug("Sensors: {}", sensors.length); 058 if (sensors.length >= 1 && sensors[0] != null) { 059 log.debug("Sensor[0]: {},{},{}", sensors[0].x, sensors[0].y, sensors[0].z); 060 } 061 if (sensors.length >= 2 && sensors[1] != null) { 062 log.debug("Sensor[1]: {},{},{}", sensors[1].x, sensors[1].y, sensors[1].z); 063 } 064 } 065 066 prep(r); 067 068 RetVal result = RPSpos(nr, Tr, Xr, Yr, Zr, Vs, Xt, Yt, Zt); 069 Xt = result.x; 070 Yt = result.y; 071 Zt = result.z; 072 Vs = result.vs; 073 074 log.debug("x = {} y = {} z0 = {} code = {}", Xt, Yt, Zt, result.code); 075 return new Measurement(r, Xt, Yt, Zt, Vs, result.code, "Ash2_1Algorithm"); 076 } 077 078 /** 079 * Seed the conversion using an estimated position 080 */ 081 @Override 082 public Measurement convert(Reading r, Point3d guess) { 083 this.Xt = guess.x; 084 this.Yt = guess.y; 085 this.Zt = guess.z; 086 087 return convert(r); 088 } 089 090 /** 091 * Seed the conversion using a last measurement 092 */ 093 @Override 094 public Measurement convert(Reading r, Measurement last) { 095 if (last != null) { 096 this.Xt = last.getX(); 097 this.Yt = last.getY(); 098 this.Zt = last.getZ(); 099 } 100 101 // if the last measurement failed, set back to origin 102 if (this.Xt > 9.E99) { 103 this.Xt = 0; 104 } 105 if (this.Yt > 9.E99) { 106 this.Yt = 0; 107 } 108 if (this.Zt > 9.E99) { 109 this.Zt = 0; 110 } 111 112 return convert(r); 113 } 114 115// RPS POSITION SOLVER Version 2.1 by R. C. Ashenfelter 02-02-07 116// Return values modified 07-10-07 117 118 /* 119 * This algorithm was provided by Robert Ashenfelter 120 * who provides no guarantee as to its usability, 121 * correctness nor intellectual property status. 122 * Use it at your own risk. 123 */ 124 int offset = 0; // Offset (usec), add to delay 125 126 @SuppressFBWarnings(value = "MS_SHOULD_BE_FINAL") // for script access 127 static public int TMAX = 35000; // Max. allowable delay (usec) 128 129 @SuppressFBWarnings(value = "MS_SHOULD_BE_FINAL") // for script access 130 static public int TMIN = 150; // Min. allowable delay (usec) 131 132 @SuppressFBWarnings(value = "MS_SHOULD_BE_FINAL") // for script access 133 static public int SMAX = 30; // Max. OK std. dev. (usec) 134 135 @SuppressFBWarnings(value = "MS_SHOULD_BE_FINAL") // for script access 136 static public int NMAX = 50; // Max. no. of receivers used 137 138 // Compute RPS Position using 139 RetVal RPSpos(int nr, double Tr[], double Xr[], double Yr[], double Zr[],// many 140 double Vs, double Xt, double Yt, double Zt) {// receivers 141 142 int i, j, jmax, k, ns, nss, nxx, nox, tov, S, cmax; 143 int[] ce = new int[NMAX]; 144 145 double Rq; 146 double[] Rs = new double[NMAX]; 147 double[] Xs = new double[NMAX]; 148 double[] Ys = new double[NMAX]; 149 double[] Zs = new double[NMAX]; 150 151 double x, y, z, Rmax; 152 double Ww, Xw, Yw, Zw, w, q; 153 double err, emax, thr, var, vmax, vmin, vold; 154 155 j = k = jmax = nox = 0; 156 w = 0; 157 var = 0; 158 159 vmax = SMAX * SMAX * Vs * Vs; 160 vmin = 1.0 * Vs * Vs; 161 ns = 0; 162 Rmax = Vs * TMAX; 163 Rs[NMAX - 1] = TMAX;// Sort receivers by delay 164 for (i = 0; i < nr; i++) { 165 if (Tr[i] == 0.0) { 166 continue;// Discard failures 167 } 168 Rq = Vs * (Tr[i] + offset);// Compute range from delay 169 if ((Rq >= Rmax) || (Rq < Vs * TMIN)) { 170 continue;// Discard too long or short 171 } 172 if (ns == 0) { 173 Rs[0] = Rq; 174 Xs[0] = Xr[i]; 175 Ys[0] = Yr[i]; 176 Zs[0] = Zr[i]; 177 ns = 1; 178 }// 1st entry 179 else { 180 j = ((ns == NMAX) ? (ns - 1) : (ns++));// Keep no more than NMAX 181 for (;; j--) {// Bubble sort 182 if ((j > 0) && (Rq < Rs[j - 1])) { 183 Rs[j] = Rs[j - 1]; 184 Xs[j] = Xs[j - 1];// Move old entries 185 Ys[j] = Ys[j - 1]; 186 Zs[j] = Zs[j - 1]; 187 } else { 188 if ((j < NMAX - 1) || (Rq < Rs[j])) {// Insert new entry 189 Rs[j] = Rq; 190 Xs[j] = Xr[i]; 191 Ys[j] = Yr[i]; 192 Zs[j] = Zr[i]; 193 } 194 break; 195 } 196 } 197 } 198 } 199 200 for (i = 0; i < ns; i++) { 201 ce[i] = 0;// Reject gross errors 202 } 203 for (i = 0; i < ns - 1; i++) { 204 for (j = i + 1; j < ns; j++) { 205 q = Math.sqrt((Xs[i] - Xs[j]) * (Xs[i] - Xs[j]) 206 + (Ys[i] - Ys[j]) * (Ys[i] - Ys[j]) + (Zs[i] - Zs[j]) * (Zs[i] - Zs[j])); 207 if ((Rs[i] + Rs[j] < q) || (Rs[i] - Rs[j] > q) || (Rs[j] - Rs[i] > q)) { 208 ++ce[i]; 209 ++ce[j]; 210 } 211 } 212 }// Count them 213 cmax = 1; 214 nxx = 0; 215 while (cmax != 0) {// Repetitively discard 216 cmax = 0;// worst offender 217 for (i = 0; i < ns; i++) { 218 if (ce[i] >= cmax) { 219 if (ce[i] > 0) { 220 nxx = ((ce[i] == cmax) ? nxx + 1 : 1); 221 } 222 cmax = ce[i]; 223 j = i; 224 } 225 }// Find it 226 if (cmax > 0) { 227 for (i = 0; i < ns; i++) {// Adjust remaining counts 228 if (i == j) { 229 continue; 230 } 231 q = Math.sqrt((Xs[i] - Xs[j]) * (Xs[i] - Xs[j]) 232 + (Ys[i] - Ys[j]) * (Ys[i] - Ys[j]) + (Zs[i] - Zs[j]) * (Zs[i] - Zs[j])); 233 if ((Rs[i] + Rs[j] < q) || (Rs[i] - Rs[j] > q) || (Rs[j] - Rs[i] > q)) { 234 --ce[i]; 235 } 236 }// Adjustment 237 for (i = j; i < ns - 1; i++) {// Discard gross error 238 Rs[i] = Rs[i + 1]; 239 Xs[i] = Xs[i + 1]; 240 Ys[i] = Ys[i + 1]; 241 Zs[i] = Zs[i + 1];// Move old entries 242 ce[i] = ce[i + 1]; 243 } 244 --ns; 245 } 246 }// One less receiver 247 nss = ns; 248 249 if (ns < 3) {// Failed: 250 Xt = Yt = Zt = 9.9999999e99; 251 return new RetVal(0, Xt, Yt, Zt, Vs); 252 }// Too few usable receivers 253 254 S = i = tov = 0; 255 x = y = 0.0; 256 z = -100000.0;// Iterative solution 257 while (S < 4) { 258 if (S == 0) {// Stage 0 259 j = k = i % ns;// Receivers in order 260 w = 1.0; 261 }// No wgts. No "All-Tog." 262 else if (S == 1) {// Stage 1 263 while ((j = (int) Math.floor((ns) * Math.random())) == k) { 264 // Receivers random order 265 } 266 k = j; 267 w = 1.0; 268 }// No weights 269 else if (S == 2) {// Stage 2 270 --k; 271 j = k % ns;// Receivers reverse order 272 w = 1.0 - Rs[j] / Rmax; 273 w = w * w;// Weight by distance 274 w *= 0.01 * (k + 1); 275 } // with fade out 276 else if (S == 3) {// Stage 3 277 }// No "One-at-a-time" 278 279 if (S < 3) {// One-At-A-Time iteration 280 q = Math.sqrt((Xs[j] - x) * (Xs[j] - x) + (Ys[j] - y) * (Ys[j] - y) + (Zs[j] - z) * (Zs[j] - z)); 281 q = w * (1.0 - Rs[j] / q);// Adjustment factor 282 x += q * (Xs[j] - x);// Position adjustments 283 y += q * (Ys[j] - y); 284 z += q * (Zs[j] - z); 285 ++i; 286 } 287 288 if (((S == 1) && (i % 50 == 0)) || ((S == 2) && (k == 0)) || (S == 3)) { 289 Ww = Xw = Yw = Zw = emax = 0.0;// All-Together iteration 290 vold = var; 291 var = 0.0; 292 for (j = 0; j < ns; j++) {// For all receivers 293 q = Math.sqrt((Xs[j] - x) * (Xs[j] - x) + (Ys[j] - y) * (Ys[j] - y) + (Zs[j] - z) * (Zs[j] - z)); 294 err = q - Rs[j]; 295 err = err * err;// Residual error 296 q = 1.0 - Rs[j] / q;// Adjustment factor 297 if (S >= 2) { 298 w = 1.0 - Rs[j] / Rmax; 299 w = w * w; 300 }// Weight by distance 301 else { 302 w = 1.0; 303 } 304 Xw += w * (x + q * (Xs[j] - x));// Accumulate averages 305 Yw += w * (y + q * (Ys[j] - y)); 306 Zw += w * (z + q * (Zs[j] - z)); 307 Ww += w; 308 var += w * err; 309 if (w * err > emax) {// Capture max. outlier 310 emax = w * err; 311 jmax = j; 312 } 313 } 314 x = Xw / Ww; 315 y = Yw / Ww; 316 z = Zw / Ww;// Avg. adjusted position 317 var = var / Ww; 318 i += ns; 319 thr = (10.0 - 30.0 / ns) * Ww / ns;// Outlier threshold 320 if ((S >= 2) && (ns > 3) && (((ns > 4) && (emax > var * thr)) || (var > 3 * vmax))) { 321 tov = ((emax > var * thr) ? 0 : 1); 322 --ns; 323 nox = 0;// If outlier too big 324 Rs[jmax] = Rs[ns]; 325 Xs[jmax] = Xs[ns];// Discard outlier entry 326 Ys[jmax] = Ys[ns]; 327 Zs[jmax] = Zs[ns]; 328 } else { 329 ++nox;// No discard 330 } 331 if ((S == 1) && (((var > 0.999 * vold) && (var < 3 * vmax)) 332 || (var < vmin) || (i >= 750))) { 333 k = 200; 334 nox = 0; 335 ++S; 336 }// Advance to Stage 2 337 if ((S == 2) && (k == 0)) { 338 k = 200; 339 if (((nox >= 2) && (var > 0.999 * vold)) || (var < vmin) || (i >= 2000)) { 340 nox = 0; 341 ++S; 342 } 343 }// Advance to Stage 3 344 if ((S == 3) && (((nox >= 1 + 110 / (ns + 5)) && (var > 0.999 * vold)) 345 || (var < 0.1 * vmin) || (i >= 2500 - ns))) { 346 ++S; 347 } 348 }// Advance to done... 349 350 if ((S == 0) && (i >= 50)) { 351 k = j; 352 var = 9e9; 353 ++S; 354 } 355 }// Advance to Stage 1 356 357 Xt = x; 358 Yt = y; 359 Zt = z;// Computed position 360 if ((var > vmax) || ((ns == 3) && (var > vmin))) {// Failed: 361 return new RetVal(-ns, Xt, Yt, Zt, Vs); 362 } // variance too big 363 if ((ns == 3) && ((nss > 4) || (nxx > 1) || (tov != 0))) {// Questionable: uncertain 364 return new RetVal(1, Xt, Yt, Zt, Vs); 365 } // gross rejection 366 if ((ns == 4) && ((nss > 5) || ((nss == 5) && (nxx == 1) && (tov == 1)))) {// or 367 return new RetVal(2, Xt, Yt, Zt, Vs); 368 } // too many 369 if ((ns >= 5) && (nss > (3 * ns - 3) / 2)) { 370 return new RetVal(2, Xt, Yt, Zt, Vs); // outlier rejections 371 } 372 return new RetVal(ns, Xt, Yt, Zt, Vs);// Success! (probably...) 373 }// End of RPSpos() 374 375// ---------------------------------------------------- 376 /** 377 * Internal class to handle return value. 378 * 379 * More of a struct, really 380 */ 381 static class RetVal { 382 383 RetVal(int code, double x, double y, double z, double vs) { 384 this.code = code; 385 this.x = x; 386 this.y = y; 387 this.z = z; 388 this.vs = vs; 389 } 390 int code; 391 @SuppressFBWarnings(value = "UUF_UNUSED_FIELD") 392 double x, y, z, t, vs; 393 } 394 395 private final static Logger log = LoggerFactory.getLogger(Ash2_1Algorithm.class); 396 397}