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}