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}