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 2nd 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 * Here is a summary of the features of the new program from Robert Ashenfelter: 022 * 023 * <ol> 024 * <li> It is completely iterative. No more exact solutions for sets of three 025 * receivers. No more weighted averages of such solutions. 026 * 027 * <li> Although both the old and the new versions can accept an unlimited 028 * number of receivers, the old version only processes a maximum of 15 while the 029 * new version processes up to 50. 030 * 031 * <li> The accuracy of the new version is approximately the same as for the old 032 * version, perhaps marginally better. However for more than 15 receivers it is 033 * significantly better. 034 * 035 * <li> It has been designed to specifically reject receiver measurements with 036 * gross errors, i.e. those which are so large that there is no possible 037 * position solution when they are combined with other measurements. It does so 038 * much better than version 1.1. (However, version 1.1 has deficiencies in this 039 * regard and is not as good at this as version 1.0.) 040 * 041 * <li> It is slightly faster. 042 * </ol> 043 * 044 * Here is a description of the new program. 045 * <p> 046 * As before the first thing it does is sort the receivers in order of 047 * increasing time delay, discarding those that failed or are too far or too 048 * near, and using the closest ones. There is a maximum that are used, now set 049 * at 50. 050 * <p> 051 * Next it discards those receivers with gross measurement errors. All possible 052 * pairs of receivers are checked to see if the sum of their measured ranges is 053 * less than, or the difference is greater than, the distance between the 054 * receivers. Counts are maintained for each receiver and the one with the 055 * largest count is booted out. The proceedure is repeated until there are no 056 * more failures. If fewer than three receivers are left there can be no 057 * solution and an error code is returned. 058 * <p> 059 * Two iterative techniques are used which I call "One-At-A-Time" and 060 * "All-Together." The first looks at one receiver at a time and moves the 061 * estimated position directly toward or away from it such that the distance is 062 * equal to the measured value. This simple technique usually converges quite 063 * rapidly. The second technique accumulates the adjustments for all receivers 064 * and then computes and applies an average for all. It is not as fast but is 065 * ultimately more accurate. 066 * <p> 067 * The solution proceeds in four stages, the first two of which are like the 068 * preliminary solution in version 1.1. Stage 0 does 50 One-At-A-Time iterations 069 * with the receivers in the sorted order. As in version 1.1, it starts from a 070 * position far, far below any likely final point. Stage 1 continues with the 071 * receivers chosen at random until it has iterated 1000 times. The procedure 072 * usually converges in 20-50 iterations, however for occasional positions the 073 * convergence is much slower. The random order is used because the procedure 074 * was occasionally observed to get stuck in a loop when using a repetitive 075 * fixed order. 076 * <p> 077 * Stage 2 continues the One-At-A-Time technique for an additional 250 078 * iterations with the receivers in reverse order ending with the closest 079 * receiver. Weights are applied assuming that close measurements are more 080 * accurate than distant ones. The weights fade out during the stage so that at 081 * the end the adjustments are very small. This fade-out fixes a problem with 082 * the One-At-A-Time technique in that it gives undue weight to the last 083 * receiver. The result at this point is quite good and the program could well 084 * stop here but it doesn't. Stage 3 runs the All-Together iteration 15 times, 085 * also using weights according to distance, to produce a more refined result. 086 * <p> 087 * The program always runs through all the iterations regardless of how fast or 088 * slow the solution converges. Only at the end does it compute the variance of 089 * the residuals (differences between measured receiver distances and those from 090 * the computed position) to check the result. The execution time ranges from 091 * 0.8 millisecond with 3 receivers to 1.3 millisecond with 50 or more receivers 092 * (1.0 GHz Pentium III). 093 * <p> 094 * Input/output is the same as for versions 1.0 and 1.1. As before, the function 095 * returns 0 if all seems well and 1 if there are fewer than 3 usable receivers 096 * (with the reported position outside the known universe). A return value of 2 097 * indicates that the variance of the residuals exceeds a fixed threshold so the 098 * reported position is questionable. The threshold is set at 30 microseconds 099 * which is equivalent to a standard deviation of 0.4 inch or 1.0 cm. This is 100 * about as small as I dare set it. Usually the reported position is garbage 101 * (because of errors in the input data) when the return value is 2, but it 102 * could be close if the input is merely excessively noisy. Likewise, the 103 * reported position is usally OK when the return value is 0 but this cannot be 104 * guaranteed. After all, errors in the data could happen to mimic good values 105 * for a wrong position. These return values tend to less reliable when the 106 * program is overloaded with too many large errors. 107 * <p> 108 * The restrictions on the configuration of transmitters and receivers, 109 * necessary to prevent the program from reporting a spurious position, are the 110 * same as those for version 1.1. 111 * <p> 112 * As before, I have tested the program with a large number of different 113 * receiver configurations having from 3 to 100 receivers and with many 114 * transmitter locations. In addition to small random measurement errors, I have 115 * added the simulation of large errors to the tests. To simulate such an error, 116 * I pick a random receiver and replace its time delay with a random value 117 * between 0 and 35,000 microseconds (equivalent to 0 to 40 feet). Depending on 118 * the configuration, this may result in a gross error or the error may be too 119 * small for this but still so large as to cause the solution to fail. Some 120 * results for four receivers and one large error are that with Larry's small 121 * initial test layout the program rejects the error and computes a correct 122 * position 90% of the time, while with a more-typical layout it may be only 123 * 50%. Performance improves to 80% correct for the typical layout with six 124 * receivers. 125 * 126 * @author Robert Ashenfelter Copyright (C) 2007 127 * @author Bob Jacobsen Copyright (C) 2007 128 */ 129public class Ash2_0Algorithm extends AbstractCalculator { 130 131 public Ash2_0Algorithm(Point3d[] sensors, double vsound, int offset) { 132 this(sensors, vsound); 133 this.offset = offset; 134 } 135 136 public Ash2_0Algorithm(Point3d[] sensors, double vsound) { 137 this.sensors = Arrays.copyOf(sensors, sensors.length); 138 this.Vs = vsound; 139 140 // load the algorithm variables 141 //Point3d origin = new Point3d(); // defaults to 0,0,0 142 } 143 144 public Ash2_0Algorithm(Point3d sensor1, Point3d sensor2, Point3d sensor3, double vsound) { 145 this(new Point3d[]{sensor1, sensor2, sensor3}, vsound); 146 } 147 148 public Ash2_0Algorithm(Point3d sensor1, Point3d sensor2, Point3d sensor3, Point3d sensor4, double vsound) { 149 this(new Point3d[]{sensor1, sensor2, sensor3, sensor4}, vsound); 150 } 151 152 double Vs; 153 double Xt = 0.0; 154 double Yt = 0.0; 155 double Zt = 0.0; 156 157 @Override 158 public Measurement convert(Reading r) { 159 160 prep(r); 161 162 RetVal result = RPSpos(nr, Tr, Xr, Yr, Zr, Vs, Xt, Yt, Zt); 163 Xt = result.x; 164 Yt = result.y; 165 Zt = result.z; 166 Vs = result.vs; 167 168 // must convert to new code 169 int code; 170 switch (result.code) { 171 case 0: // ok 172 code = 4; 173 break; 174 case 1: // less than 3 receivers 175 code = 0; 176 break; 177 case 2: // variance too large 178 code = -Tr.length; 179 break; 180 default: 181 log.error("Unexpected error code: {}", result.code); 182 code = 0; 183 } 184 log.debug("old code={} new code={}", result.code, code); 185 186 log.debug("x = {} y = {} z0 = {} code = {}", Xt, Yt, Zt, code); 187 return new Measurement(r, Xt, Yt, Zt, Vs, code, "Ash2_0Algorithm"); 188 } 189 190 /** 191 * Seed the conversion using an estimated position 192 */ 193 @Override 194 public Measurement convert(Reading r, Point3d guess) { 195 this.Xt = guess.x; 196 this.Yt = guess.y; 197 this.Zt = guess.z; 198 199 return convert(r); 200 } 201 202 /** 203 * Seed the conversion using a last measurement 204 */ 205 @Override 206 public Measurement convert(Reading r, Measurement last) { 207 if (last != null) { 208 this.Xt = last.getX(); 209 this.Yt = last.getY(); 210 this.Zt = last.getZ(); 211 } 212 213 // if the last measurement failed, set back to origin 214 if (this.Xt > 9.E99) { 215 this.Xt = 0; 216 } 217 if (this.Yt > 9.E99) { 218 this.Yt = 0; 219 } 220 if (this.Zt > 9.E99) { 221 this.Zt = 0; 222 } 223 224 return convert(r); 225 } 226 227// ---------------------------------------------------- 228// RPS POSITION SOLVER Version 2.0 by R. C. Ashenfelter 01-14-07 229 230 /* 231 * This algorithm was provided by Robert Ashenfelter 232 * who provides no guarantee as to its usability, 233 * correctness nor intellectual property status. 234 * Use it at your own risk. 235 */ 236 int offset = 0; // Offset (usec), add to delay 237 238 final static int TMAX = 35000; // Max. allowable delay (usec) 239 final static int TMIN = 150; // Min. allowable delay (usec) 240 final static int SMAX = 30; // Max. OK std. dev. (usec) 241 final static int NMAX = 50; // Max. no. of receivers used 242 243 // Compute RPS Position using 244 @SuppressFBWarnings(value = "IP_PARAMETER_IS_DEAD_BUT_OVERWRITTEN") // it's secretly FORTRAN.. 245 RetVal RPSpos(int nr, double Tr[], double Xr[], double Yr[], double Zr[],// many 246 double Vs, double Xt, double Yt, double Zt) {// receivers 247 248 int i, j, k, ns, cmax; 249 int[] ce = new int[NMAX]; 250 double Rq; 251 double[] Rs = new double[NMAX]; 252 double[] Xs = new double[NMAX]; 253 double[] Ys = new double[NMAX]; 254 double[] Zs = new double[NMAX]; 255 double x, y, z, Rmax; 256 double Ww, Xw, Yw, Zw, w, q; 257 double err, var, vmax, vmin; 258 259 j = k = 0; 260 var = 0; 261 262 vmax = SMAX * SMAX * Vs * Vs; 263 vmin = 1.0 * Vs * Vs; 264 ns = 0; 265 Rs[NMAX - 1] = TMAX; 266 Rmax = Vs * TMAX;// Sort receivers by delay 267 for (i = 0; i < nr; i++) { 268 if (Tr[i] == 0.0) { 269 continue;// Discard failures 270 } 271 Rq = Vs * (Tr[i] + offset);// Compute range from delay 272 if ((Rq >= Rmax) || (Rq < Vs * TMIN)) { 273 continue;// Discard too long or short 274 } 275 if (ns == 0) { 276 Rs[0] = Rq; 277 Xs[0] = Xr[i]; 278 Ys[0] = Yr[i]; 279 Zs[0] = Zr[i]; 280 ns = 1; 281 }// 1st entry 282 else { 283 j = ((ns == NMAX) ? (ns - 1) : (ns++));// Keep no more than NMAX 284 for (;; j--) {// Bubble sort 285 if ((j > 0) && (Rq < Rs[j - 1])) { 286 Rs[j] = Rs[j - 1]; 287 Xs[j] = Xs[j - 1];// Move old entries 288 Ys[j] = Ys[j - 1]; 289 Zs[j] = Zs[j - 1]; 290 } else { 291 if ((j < NMAX - 1) || (Rq < Rs[j])) {// Insert new entry 292 Rs[j] = Rq; 293 Xs[j] = Xr[i]; 294 Ys[j] = Yr[i]; 295 Zs[j] = Zr[i]; 296 } 297 break; 298 } 299 } 300 } 301 } 302 303 for (i = 0; i < ns; i++) { 304 ce[i] = 0;// Reject gross errors 305 } 306 for (i = 0; i < ns - 1; i++) { 307 for (j = i + 1; j < ns; j++) { 308 q = Math.sqrt((Xs[i] - Xs[j]) * (Xs[i] - Xs[j]) 309 + (Ys[i] - Ys[j]) * (Ys[i] - Ys[j]) + (Zs[i] - Zs[j]) * (Zs[i] - Zs[j])); 310 if ((Rs[i] + Rs[j] < q) || (Rs[i] - Rs[j] > q) || (Rs[j] - Rs[i] > q)) { 311 ++ce[i]; 312 ++ce[j]; 313 } 314 } 315 }// Count them 316 cmax = 1; 317 while (cmax != 0) {// Repetitively discard 318 cmax = 0;// worst offender 319 for (i = 0; i < ns; i++) { 320 if (ce[i] >= cmax) { 321 cmax = ce[i]; 322 j = i; 323 } 324 }// Find it 325 if (cmax > 0) { 326 for (i = 0; i < ns; i++) {// Adjust remaining counts 327 if (i == j) { 328 continue; 329 } 330 q = Math.sqrt((Xs[i] - Xs[j]) * (Xs[i] - Xs[j]) 331 + (Ys[i] - Ys[j]) * (Ys[i] - Ys[j]) + (Zs[i] - Zs[j]) * (Zs[i] - Zs[j])); 332 if ((Rs[i] + Rs[j] < q) || (Rs[i] - Rs[j] > q) || (Rs[j] - Rs[i] > q)) { 333 --ce[i]; 334 } 335 }// Adjustment 336 for (i = j; i < ns - 1; i++) {// Discard gross error 337 Rs[i] = Rs[i + 1]; 338 Xs[i] = Xs[i + 1]; 339 Ys[i] = Ys[i + 1]; 340 Zs[i] = Zs[i + 1];// Move old entries 341 ce[i] = ce[i + 1]; 342 } 343 --ns; 344 } 345 }// One less receiver 346 347 if (ns < 3) {// Failed: 348 Xt = Yt = Zt = 9.9999999e99; 349 return new RetVal(1, Xt, Yt, Zt, Vs); 350 }// Too few usable receivers 351 352 x = y = 0.0; 353 z = -100000.0;// Iterative solution 354 for (i = 0; i < 1250; i++) {// One-At-A-Time iteration 355 if (i < 50) { 356 j = k = i % ns;// Stage 0 357 } else if (i < 1000) {// Receivers in order 358 while ((j = (int) Math.floor((ns) * Math.random())) == k) { 359 // Stage 1 360 } 361 k = j; 362 }// Receivers random order 363 else { 364 j = (1249 - i) % ns;// Stage 2 365 } 366 if (i < 750) { 367 w = 1.0;// Receivers reverse order 368 } else { 369 w = 1.0 - Rs[j] / Rmax; 370 w = w * w; 371 }// Weight by distance 372 if (i >= 1000) { 373 w *= 5.0 - 0.004 * i; // with fade out 374 } 375 q = Math.sqrt((Xs[j] - x) * (Xs[j] - x) + (Ys[j] - y) * (Ys[j] - y) + (Zs[j] - z) * (Zs[j] - z)); 376 q = w * (1.0 - Rs[j] / q);// Adjustment factor 377 x += q * (Xs[j] - x);// Position adjustments 378 y += q * (Ys[j] - y); 379 z += q * (Zs[j] - z); 380 } 381 382 for (i = 0; i < 15; i++) {// Stage 3 383 Ww = Xw = Yw = Zw = var = 0.0;// All-Together iteration 384 for (j = 0; j < ns; j++) {// For all receivers 385 q = Math.sqrt((Xs[j] - x) * (Xs[j] - x) + (Ys[j] - y) * (Ys[j] - y) + (Zs[j] - z) * (Zs[j] - z)); 386 err = q - Rs[j];// Residual error 387 q = 1.0 - Rs[j] / q;// Adjustment factor 388 w = 1.0 - Rs[j] / Rmax; 389 w = w * w;// Weight by distance 390 Xw += w * (x + q * (Xs[j] - x));// Accumulate averages 391 Yw += w * (y + q * (Ys[j] - y)); 392 Zw += w * (z + q * (Zs[j] - z)); 393 Ww += w; 394 var += w * err * err; 395 } 396 x = Xw / Ww; 397 y = Yw / Ww; 398 z = Zw / Ww;// Avg. adjusted position 399 var = var / Ww; 400 } 401 402 Xt = x; 403 Yt = y; 404 Zt = z;// Computed position 405 if ((var > vmax) || ((ns == 3) && (var > vmin))) { 406 return new RetVal(2, Xt, Yt, Zt, Vs); 407 }// Failed: variance too big 408 return new RetVal(0, Xt, Yt, Zt, Vs);// Success! (probably...) 409 }// End of RPSpos() 410 411// ---------------------------------------------------- 412 /** 413 * Internal class to handle return value. 414 * 415 * More of a struct, really 416 */ 417 @SuppressFBWarnings(value = "UUF_UNUSED_FIELD") // t not formally needed 418 static class RetVal { 419 420 RetVal(int code, double x, double y, double z, double vs) { 421 this.code = code; 422 this.x = x; 423 this.y = y; 424 this.z = z; 425 this.vs = vs; 426 } 427 int code; 428 double x, y, z, t, vs; 429 } 430 431 private final static Logger log = LoggerFactory.getLogger(Ash2_0Algorithm.class); 432 433}