001package jmri.jmrix.rps; 002 003import javax.vecmath.Point3d; 004import org.slf4j.Logger; 005import org.slf4j.LoggerFactory; 006 007/** 008 * Implementation of 1st algorithm for reducing Readings. 009 * <p> 010 * This algorithm was provided by Robert Ashenfelter based in part on the work 011 * of Ralph Bucher in his paper "Exact Solution for Three Dimensional Hyperbolic 012 * Positioning Algorithm and Synthesizable VHDL Model for Hardware 013 * Implementation". 014 * <p> 015 * Neither Ashenfelter nor Bucher provide any guarantee as to the intellectual 016 * property status of this algorithm. Use it at your own risk. 017 * 018 * @author Bob Jacobsen Copyright (C) 2006 019 */ 020public class InitialAlgorithm implements Calculator { 021 022 public InitialAlgorithm(Point3d[] sensors, double vsound) { 023 this(sensors[0], sensors[1], sensors[2], sensors[3], vsound); 024 } 025 026 public InitialAlgorithm(Point3d sensor1, Point3d sensor2, Point3d sensor3, double vsound) { 027 this.sensor1 = sensor1; 028 this.sensor2 = sensor2; 029 this.sensor3 = sensor3; 030 031 this.vsound = vsound; 032 033 // load the algorithm variables 034 //Point3d origin = new Point3d(); // defaults to 0,0,0 035 xi = sensor1.x; 036 yi = sensor1.y; 037 zi = sensor1.z; 038 039 xj = sensor2.x; 040 yj = sensor2.y; 041 zj = sensor2.z; 042 043 xk = sensor3.x; 044 yk = sensor3.y; 045 zk = sensor3.z; 046 047 ngps = 3; 048 } 049 050 public InitialAlgorithm(Point3d sensor1, Point3d sensor2, Point3d sensor3, Point3d sensor4, double vsound) { 051 this(sensor1, sensor2, sensor3, vsound); 052 053 this.sensor4 = sensor4; 054 055 // load the algorithm variables 056 //Point3d origin = new Point3d(); // defaults to 0,0,0 057 xl = sensor4.x; 058 yl = sensor4.y; 059 zl = sensor4.z; 060 061 ngps = 4; 062 } 063 064 double vsound; 065 066 @Override 067 public Measurement convert(Reading r) { 068 ngps = r.getNValues(); 069 ri = r.getValue(1) * vsound; 070 rj = r.getValue(2) * vsound; 071 rk = r.getValue(3) * vsound; 072 if (r.getNValues() > 3) { 073 rl = r.getValue(4) * vsound; 074 } 075 076 log.debug("inputs {} {} {} {}", ri, rj, rk, rl); 077 078 gps(); 079 080 log.debug("x = {} y = {} z0 = {}", x0, y0, z0); 081 return new Measurement(r, x0, y0, z0, vsound, -99, "Initial"); 082 } 083 084 /** 085 * Seed the conversion using an estimated position 086 */ 087 @Override 088 public Measurement convert(Reading r, Point3d guess) { 089 this.x = guess.x; 090 this.y = guess.y; 091 this.z = guess.z; 092 093 return convert(r); 094 } 095 096 /** 097 * Seed the conversion using a last measurement 098 */ 099 @Override 100 public Measurement convert(Reading r, Measurement last) { 101 if (last != null) { 102 this.x = last.getX(); 103 this.y = last.getY(); 104 this.z = last.getZ(); 105 } 106 107 // if the last measurement failed, set back to origin 108 if (this.x > 9.E99) { 109 this.x = 0; 110 } 111 if (this.y > 9.E99) { 112 this.y = 0; 113 } 114 if (this.z > 9.E99) { 115 this.z = 0; 116 } 117 118 return convert(r); 119 } 120 121 // Sensor position objects, up to 4 122 Point3d sensor1; 123 Point3d sensor2; 124 Point3d sensor3; 125 Point3d sensor4; 126 127 /** 128 * The following is the original algorithm, as provided by Ash as a C 129 * routine 130 */ 131 double x, y, z; 132 double x0, y0, z0, r0; 133 134 int ngps; 135 136// positions of sensors 137 double xi, yi, zi; 138 double xj, yj, zj; 139 double xk, yk, zk; 140 double xl, yl, zl; 141 142// distance to sensors (true input) 143 double ri, rj, rk, rl; 144 145// calculation subroutine 146 boolean gps() {// GPS Position Solver 147 double xie, yie, zie, rie, xje, yje, zje, rje; 148 double xke, yke, zke, rke, xle, yle, zle, rle;// Inputs (global variables) 149 double xij, yij, zij, rij, xik, yik, zik, rik; 150 double xjk, yjk, zjk, rjk, xkl, ykl, zkl, rkl;// ngps = # of satellites 151 double Ax, Ay, Az, Bx, By, Bz, Dx, Dy, Dz;// 3 (abs.) or 4 (rel.) 152 double Ca, Cb, Cc, Cd, Ce, Cf, Ci, Cj, Cx, Cy, Cz; 153 double r0i, r0j, r0k, r0l, r01, r02;// sat. position, range: 154 double x1, y1, z1, x2, y2, z2, e1, e2;// xi, yi, zi, ri 155 // xj, yj, zj, rj 156 // xk, yk, zk, rk 157 // xl, yl, zl, rl (rel.) 158 159 // in case of early error return, make it far away 160 x0 = y0 = z0 = r0 = 9.9999999e99; 161 162 // avoid "not initialized" errors from compiler 163 r01 = -1; 164 r02 = -1; 165 166 // calculation 167 if (ngps == 3) {// Solve with absolute ranges 168 xik = xi - xk; 169 yik = yi - yk; 170 zik = zi - zk; 171 log.debug("xik={} yik={}", xik, yik); 172 xjk = xj - xk; 173 yjk = yj - yk; 174 zjk = zj - zk; 175 log.debug("xjk={} yjk={}", xjk, yjk); 176 Ci = (xi * xi - xk * xk + yi * yi - yk * yk + zi * zi - zk * zk - ri * ri + rk * rk) / 2; 177 Cj = (xj * xj - xk * xk + yj * yj - yk * yk + zj * zj - zk * zk - rj * rj + rk * rk) / 2; 178 Dz = xik * yjk - xjk * yik; 179 Dy = zik * xjk - zjk * xik; 180 Dx = yik * zjk - yjk * zik; 181 182 if ((Math.abs(Dx) > Math.abs(Dy)) && (Math.abs(Dx) > Math.abs(Dz))) { 183 log.debug("case 1"); 184 Ay = (zik * xjk - zjk * xik) / Dx; 185 By = (zjk * Ci - zik * Cj) / Dx; 186 Az = (yjk * xik - yik * xjk) / Dx; 187 Bz = (yik * Cj - yjk * Ci) / Dx; 188 Ax = Ay * Ay + Az * Az + 1.0; 189 Bx = (Ay * (yk - By) + Az * (zk - Bz) + xk) / Ax; 190 Cx = Bx * Bx - (By * By + Bz * Bz - 2 * yk * By - 2 * zk * Bz + yk * yk + zk * zk + xk * xk - rk * rk) / Ax; 191 if (Cx < 0.0) { 192 log.warn("Cx is {}, less than 0, in 3 sensor case", Cx); 193 return false; 194 } 195 x1 = Bx + Math.sqrt(Cx); 196 y1 = Ay * x1 + By; 197 z1 = Az * x1 + Bz; 198 x2 = 2 * Bx - x1; 199 y2 = Ay * x2 + By; 200 z2 = Az * x2 + Bz; 201 } else if (Math.abs(Dy) > Math.abs(Dz)) { 202 log.debug("case 2"); 203 Az = (xik * yjk - xjk * yik) / Dy; 204 Bz = (xjk * Ci - xik * Cj) / Dy; 205 Ax = (zjk * yik - zik * yjk) / Dy; 206 Bx = (zik * Cj - zjk * Ci) / Dy; 207 Ay = Az * Az + Ax * Ax + 1.0; 208 By = (Az * (zk - Bz) + Ax * (xk - Bx) + yk) / Ay; 209 Cy = By * By - (Bz * Bz + Bx * Bx - 2 * zk * Bz - 2 * xk * Bx + zk * zk + xk * xk + yk * yk - rk * rk) / Ay; 210 if (Cy < 0.0) { 211 log.warn("Cy is {}, less than 0, in 3 sensor case", Cy); 212 return false; 213 } 214 y1 = By + Math.sqrt(Cy); 215 z1 = Az * y1 + Bz; 216 x1 = Ax * y1 + Bx; 217 y2 = 2 * By - y1; 218 z2 = Az * y2 + Bz; 219 x2 = Ax * y2 + Bx; 220 } else { 221 log.debug("case 3"); 222 if (Dz == 0.0) { 223 log.warn("Dz is 0 in 3 sensor case"); 224 return false; 225 } 226 Ax = (yik * zjk - yjk * zik) / Dz; 227 Bx = (yjk * Ci - yik * Cj) / Dz; 228 Ay = (xjk * zik - xik * zjk) / Dz; 229 By = (xik * Cj - xjk * Ci) / Dz; 230 Az = Ax * Ax + Ay * Ay + 1.0; 231 Bz = (Ax * (xk - Bx) + Ay * (yk - By) + zk) / Az; 232 Cz = Bz * Bz - (Bx * Bx + By * By - 2 * xk * Bx - 2 * yk * By + xk * xk + yk * yk + zk * zk - rk * rk) / Az; 233 if (Cz < 0.0) { 234 log.warn("Cz is {}, less than 0, in 3 sensor case", Cz); 235 return false; 236 } 237 z1 = Bz + Math.sqrt(Cz); 238 x1 = Ax * z1 + Bx; 239 y1 = Ay * z1 + By; 240 z2 = 2 * Bz - z1; 241 x2 = Ax * z2 + Bx; 242 y2 = Ay * z2 + By; 243 log.debug("x1 = {}", x1); 244 log.debug("x2 = {}", x2); 245 } 246 } else if (ngps == 4) {// Solve with relative ranges 247 xie = xi + 1e-9; 248 yie = yi - 7e-9; 249 zie = zi - 4e-9; 250 rie = ri + 6e-9; 251 xje = xj + 5e-9; 252 yje = yj - 3e-9; 253 zje = zj + 2e-9; 254 rje = rj - 8e-9; 255 xke = xk - 2e-9; 256 yke = yk - 6e-9; 257 zke = zk + 7e-9; 258 rke = rk - 5e-9; 259 xle = xl + 8e-9; 260 yle = yl + 4e-9; 261 zle = zl - 1e-9; 262 rle = rl + 3e-9; 263 xij = xie - xje; 264 xik = xie - xke; 265 xjk = xje - xke; 266 xkl = xke - xle; 267 yij = yie - yje; 268 yik = yie - yke; 269 yjk = yje - yke; 270 ykl = yke - yle; 271 zij = zie - zje; 272 zik = zie - zke; 273 zjk = zje - zke; 274 zkl = zke - zle; 275 rij = rie - rje; 276 rik = rie - rke; 277 rjk = rje - rke; 278 rkl = rke - rle; 279 Ci = (rik * (rij * rij + xie * xie - xje * xje + yie * yie - yje * yje + zie * zie - zje * zje) 280 - rij * (rik * rik + xie * xie - xke * xke + yie * yie - yke * yke + zie * zie - zke * zke)) / 2; 281 Cj = (rkl * (rjk * rjk + xke * xke - xje * xje + yke * yke - yje * yje + zke * zke - zje * zje) 282 + rjk * (rkl * rkl + xke * xke - xle * xle + yke * yke - yle * yle + zke * zke - zle * zle)) / 2; 283 Dx = rik * yij - rij * yik; 284 Dx = ((Dx != 0.0) ? Dx : 1e-12); 285 Dy = rjk * ykl - rkl * yjk; 286 Dy = ((Dy != 0.0) ? Dy : 1e-12); 287 Ca = (rij * xik - rik * xij) / Dx; 288 Cb = (rij * zik - rik * zij) / Dx; 289 Cc = Ci / Dx; 290 Cd = (rkl * xjk - rjk * xkl) / Dy; 291 Ce = (rkl * zjk - rjk * zkl) / Dy; 292 Cf = Cj / Dy; 293 Dx = Ca - Cd; 294 Dx = ((Dx != 0.0) ? Dx : 1e-12); 295 Ax = (Ce - Cb) / Dx; 296 Bx = (Cf - Cc) / Dx; 297 Ay = (Ca * Ax) + Cb; 298 By = (Ca * Bx) + Cc; 299 Ci = rik * rik + xie * xie - xke * xke + yie * yie - yke * yke 300 + zie * zie - zke * zke - 2 * Bx * xik - 2 * By * yik; 301 Cj = 2 * (Ax * xik + Ay * yik + zik); 302 Dz = 4 * rik * rik * (Ax * Ax + Ay * Ay + 1) - Cj * Cj; 303 Dz = ((Dz != 0.0) ? Dz : 1e-12); 304 Bz = (4 * rik * rik * (Ax * (xie - Bx) + Ay * (yie - By) + zie) - Ci * Cj) / Dz; 305 Cz = Math.abs(Bz * Bz 306 - (4 * rik * rik * ((xie - Bx) * (xie - Bx) + (yie - By) * (yie - By) + zie * zie) - Ci * Ci) / Dz); 307 z1 = Bz + Math.sqrt(Cz); 308 x1 = Ax * z1 + Bx; 309 y1 = Ay * z1 + By; 310 z2 = 2 * Bz - z1; 311 x2 = Ax * z2 + Bx; 312 y2 = Ay * z2 + By; 313 314 r0i = Math.sqrt((xi - x1) * (xi - x1) + (yi - y1) * (yi - y1) + (zi - z1) * (zi - z1)) - ri;// Check 315 r0j = Math.sqrt((xj - x1) * (xj - x1) + (yj - y1) * (yj - y1) + (zj - z1) * (zj - z1)) - rj;// solu- 316 r0k = Math.sqrt((xk - x1) * (xk - x1) + (yk - y1) * (yk - y1) + (zk - z1) * (zk - z1)) - rk;// tions 317 r0l = Math.sqrt((xl - x1) * (xl - x1) + (yl - y1) * (yl - y1) + (zl - z1) * (zl - z1)) - rl; 318 r01 = (r0i + r0j + r0k + r0l) / 4; 319 e1 = Math.sqrt(((r0i - r01) * (r0i - r01) + (r0j - r01) * (r0j - r01) 320 + (r0k - r01) * (r0k - r01) + (r0l - r01) * (r0l - r01)) / 4); 321 log.debug("e1 = {}", e1); 322 if (e1 > 1e-4) { 323 x1 = y1 = z1 = r01 = 9.9999999e99; // solution 1 NG 324 } 325 r0i = Math.sqrt((xi - x2) * (xi - x2) + (yi - y2) * (yi - y2) + (zi - z2) * (zi - z2)) - ri; 326 r0j = Math.sqrt((xj - x2) * (xj - x2) + (yj - y2) * (yj - y2) + (zj - z2) * (zj - z2)) - rj; 327 r0k = Math.sqrt((xk - x2) * (xk - x2) + (yk - y2) * (yk - y2) + (zk - z2) * (zk - z2)) - rk; 328 r0l = Math.sqrt((xl - x2) * (xl - x2) + (yl - y2) * (yl - y2) + (zl - z2) * (zl - z2)) - rl; 329 r02 = (r0i + r0j + r0k + r0l) / 4; 330 e2 = Math.sqrt(((r0i - r02) * (r0i - r02) + (r0j - r02) * (r0j - r02) 331 + (r0k - r02) * (r0k - r02) + (r0l - r02) * (r0l - r02)) / 4); 332 log.debug("e2 = {}", e2); 333 if (e2 > 1e-4) { 334 x2 = y2 = z2 = r02 = 9.9999999e99; // solution 2 NG 335 } 336 } else { // Invalid value of ngps 337 log.warn("ngps no good: {}", ngps); 338 return false; 339 } 340 341 e1 = (x - x1) * (x - x1) + (y - y1) * (y - y1) + (z - z1) * (z - z1);// Pick solution closest 342 e2 = (x - x2) * (x - x2) + (y - y2) * (y - y2) + (z - z2) * (z - z2);// to x, y, z 343 if (e1 <= e2) {// (also global inputs) 344 x0 = x1; 345 y0 = y1; 346 z0 = z1; 347 r0 = r01; 348 }//Solution (global variables) 349 else {// GPS Position = 350 x0 = x2; 351 y0 = y2; 352 z0 = z2; 353 r0 = r02; 354 }// x0, y0, z0, 355 return true;// r0 = range offset (rel) 356 } 357 358 private final static Logger log = LoggerFactory.getLogger(InitialAlgorithm.class); 359 360}