Source code

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 RPS location-finding using GPS equations from Sam Storm van
011 * Leeuwen {@literal <samsvl@nlr.nl>}, ported to Java by Norris Weimer
012 * {@literal <norris.weimer@ualberta.ca>}, and ported to JMRI/RPS by Bob
013 * Jacobsen.
014 *
015 * The original Pascal code and documentation is on these web pages
016 * <a href="http://callisto.worldonline.nl/~samsvl/stdalone.pas">http://callisto.worldonline.nl/~samsvl/stdalone.pas</a>
017 * <a href="http://callisto.worldonline.nl/~samsvl/satpos.htm">http://callisto.worldonline.nl/~samsvl/satpos.htm</a>
018 * <a href="http://callisto.worldonline.nl/~samsvl/stdalone.htm">http://callisto.worldonline.nl/~samsvl/stdalone.htm</a>
019 * There is also a link there to a C port of Sam's programs.
020 *
021 * @author Bob Jacobsen Copyright (C) 2008
022 */
023public class Analytic_AAlgorithm extends AbstractCalculator {
024
025    public Analytic_AAlgorithm(Point3d[] sensors, double vsound, int offset) {
026        this(sensors, vsound);
027        this.offset = offset;
028    }
029
030    public Analytic_AAlgorithm(Point3d[] sensors, double vsound) {
031        this.sensors = Arrays.copyOf(sensors, sensors.length);
032        this.Vs = vsound;
033
034        // load the algorithm variables
035        //Point3d origin = new Point3d(); // defaults to 0,0,0
036    }
037
038    public Analytic_AAlgorithm(Point3d sensor1, Point3d sensor2, Point3d sensor3, double vsound) {
039        this(new Point3d[]{sensor1, sensor2, sensor3}, vsound);
040    }
041
042    public Analytic_AAlgorithm(Point3d sensor1, Point3d sensor2, Point3d sensor3, Point3d sensor4, double vsound) {
043        this(new Point3d[]{sensor1, sensor2, sensor3, sensor4}, vsound);
044    }
045
046    double Vs;
047    double Xt = 0.0;
048    double Yt = 0.0;
049    double Zt = 0.0;
050
051    @Override
052    public Measurement convert(Reading r) {
053
054        if (log.isDebugEnabled()) {
055            log.debug("Reading: {}", r.toString());
056            log.debug("Sensors: {}", sensors.length);
057            if (sensors.length >= 1 && sensors[0] != null) {
058                log.debug("Sensor[0]: {},{},{}", sensors[0].x, sensors[0].y, sensors[0].z);
059            }
060        }
061
062        log.debug("There should have been an earth-shattering kaboom!");
063        // RetVal result = RPSpos(nr, Tr, Xr, Yr, Zr, Vs, Xt, Yt, Zt);
064
065        double[][] Xs = new double[33][3];
066
067        boolean[] SV = new boolean[33];
068        for (int i = 0; i < 33; i++) {
069            SV[i] = false;
070        }
071
072        double[] P = new double[33];
073        double[] Xr = new double[3];
074        Xr[0] = Xr[1] = Xr[2] = 0;
075
076        nr = r.getNValues();
077        if (nr != sensors.length - 1) {
078            log.error("Mismatch: {} readings, {} receivers", nr, sensors.length - 1);
079        }
080        nr = Math.min(nr, sensors.length - 1); // accept the shortest
081
082        // generate vectors
083        int j = 0;
084        for (int i = 0; i <= nr; i++) {
085            if (sensors[i] != null) {
086                P[j] = r.getValue(i) * Vs;
087                Xs[j][0] = sensors[i].x;
088                Xs[j][1] = sensors[i].y;
089                Xs[j][2] = sensors[i].z;
090                SV[j] = true;
091                log.debug("  {}th point at {},{},{} time={} is distance {}", j, Xs[j][0], Xs[j][1], Xs[j][2], r.getValue(i), P[j]);
092                j++;
093            }
094        }
095        nr = j;
096        log.debug("nr is {}", nr);
097
098        double[] result = solve(Xs, SV, P, Xr);
099
100        if (result == null) {
101            log.error("failed to converge");
102            return new Measurement(r, -99999., -99999., -99999., Vs, -20, "Analytic_A");
103
104        }
105
106        Xt = result[0];
107        Yt = result[1];
108        Zt = result[2];
109
110        // Vs = result.vs;
111        log.debug("Result x = {} y = {} z0 = {} time offset={}", Xt, Yt, Zt, result[3]);
112        return new Measurement(r, Xt, Yt, Zt, Vs, nr, "Analytic_A");
113    }
114
115    /**
116     * Seed the conversion using an estimated position
117     */
118    @Override
119    public Measurement convert(Reading r, Point3d guess) {
120        this.Xt = guess.x;
121        this.Yt = guess.y;
122        this.Zt = guess.z;
123
124        return convert(r);
125    }
126
127    /**
128     * Seed the conversion using a last measurement
129     */
130    @Override
131    public Measurement convert(Reading r, Measurement last) {
132        if (last != null) {
133            this.Xt = last.getX();
134            this.Yt = last.getY();
135            this.Zt = last.getZ();
136        }
137
138        // if the last measurement failed, set back to origin
139        if (this.Xt > 9.E99) {
140            this.Xt = 0;
141        }
142        if (this.Yt > 9.E99) {
143            this.Yt = 0;
144        }
145        if (this.Zt > 9.E99) {
146            this.Zt = 0;
147        }
148
149        return convert(r);
150    }
151
152    int offset = 0;
153
154    /**
155     * *************************************************************************
156     *
157     * @param Xs array with 3 columns and 32 rows, for the coordinates of the
158     *           sat's
159     * @param SV valid prn's
160     * @param P  pseudoranges
161     *
162     * (note: arrays actually have 33 rows, but row 0 is unused, in order to
163     * index by actual prn number)
164     *
165     * @param Xr input of initial guess ( user position in ECEF)
166     * @return [X, X, X, Cr] output of final position and receiver clock error
167     *         return null if calculation failed //do: throw exception instead
168     */
169    public double[] solve(double[][] Xs, boolean[] SV, double[] P, double[] Xr) {
170
171        double[] R = new double[33];
172        double[] L = new double[33];
173        double[][] A = new double[33][4];
174        double[] AL = new double[4];
175        double[][] AA = new double[4][4];
176        double[][] AAi = new double[4][4];
177        double det;
178        double[] D = new double[5];
179
180        int it = 0; //iteration counter
181        do {
182            it++;
183
184            for (int prn = 1; prn <= 32; prn++) {
185                if (SV[prn]) {
186                    //range from receiver to satellite
187                    R[prn] = Math.sqrt((Xr[0] - Xs[prn][0]) * (Xr[0] - Xs[prn][0])
188                            + (Xr[1] - Xs[prn][1]) * (Xr[1] - Xs[prn][1])
189                            + (Xr[2] - Xs[prn][2]) * (Xr[2] - Xs[prn][2]));
190                    //range residual value
191                    L[prn] = P[prn] - R[prn];
192                    //A is the geometry matrix or model matrix
193                    for (int k = 0; k < 3; k++) {
194                        A[prn][k] = (Xr[k] - Xs[prn][k]) / R[prn];
195                    }
196                    A[prn][3] = -1.0;
197                }
198            }
199
200            //calculate A.L
201            for (int k = 0; k <= 3; k++) {
202                AL[k] = 0.0;
203                for (int prn = 1; prn <= 32; prn++) {
204                    if (SV[prn]) {
205                        AL[k] += A[prn][k] * L[prn];
206                    }
207                }
208            }
209
210            //calculate A.A
211            for (int k = 0; k <= 3; k++) {
212                for (int i = 0; i <= 3; i++) {
213                    AA[k][i] = 0.0;
214                    for (int prn = 1; prn <= 32; prn++) {
215                        if (SV[prn]) {
216                            AA[k][i] += A[prn][k] * A[prn][i];
217                        }
218                    }
219                }
220            }
221
222            //invert A.A
223            //do: use a better procedure to solve these equations
224            det = AA[0][0] * sub(AA, 0, 0) - AA[1][0] * sub(AA, 1, 0)
225                    + AA[2][0] * sub(AA, 2, 0) - AA[3][0] * sub(AA, 3, 0);
226            if (det == 0.0) {
227                return null;
228            }
229
230            int j;
231            int n;
232            for (int k = 0; k <= 3; k++) {
233                for (int i = 0; i <= 3; i++) {
234                    n = k + i;
235                    if (n % 2 != 0) //was:    odd(n) 
236                    {
237                        j = -1;
238                    } else {
239                        j = 1;
240                    }
241                    AAi[k][i] = j * sub(AA, i, k) / det;
242                }
243            }
244
245            //calculate (invA.A).(A.L)
246            for (int k = 0; k <= 3; k++) {
247                D[k] = 0.0;
248                for (int i = 0; i <= 3; i++) {
249                    D[k] += AAi[k][i] * AL[i];
250                }
251            }
252
253            //update position
254            for (int k = 0; k < 3; k++) {
255                Xr[k] += D[k];
256            }
257
258            // display how close
259            if (log.isDebugEnabled()) {
260                log.debug("  after {}, delta is {}", it, Math.abs(D[0]) + Math.abs(D[1]) + Math.abs(D[2]));
261            }
262        } while ((it < 6) //there is something wrong if more than 6 iterations are required
263                && ((Math.abs(D[0]) + Math.abs(D[1]) + Math.abs(D[2])) >= 1.0E-2));  //iteration criterion
264
265        double Cr = D[3]; //receiver clock error
266
267        if (it >= 6) {
268            log.error("Can't solve, iteration limit exceeded.  it = {}", it);
269            return null;
270        }
271
272        return new double[]{Xr[0], Xr[1], Xr[2], Cr};
273    }
274
275    /**
276     * *************************************************************************
277     * finds the determinant of a minor of a 4 x 4 matrix
278     *
279     * @param A input 4 x 4 array
280     * @param r the row to be deleted
281     * @param c the column to be deleted
282     * @return subdet determinant of the resulting 3 x 3 matrix
283     */
284    public double sub(double[][] A, int r, int c) {
285
286        double[][] B = new double[3][3];
287        int i1, j1;
288
289        //note: I changed how this part of the function was coded    - NW
290        for (int i = 0; i < 3; i++) {
291            i1 = i;
292            if (i >= r) {
293                i1++;
294            }
295            for (int j = 0; j < 3; j++) {
296                j1 = j;
297                if (j >= c) {
298                    j1++;
299                }
300                B[i][j] = A[i1][j1];
301            }
302        }
303
304        double subdet = B[0][0] * (B[1][1] * B[2][2] - B[1][2] * B[2][1])
305                - B[1][0] * (B[0][1] * B[2][2] - B[2][1] * B[0][2])
306                + B[2][0] * (B[0][1] * B[1][2] - B[0][2] * B[1][1]);
307
308        return subdet;
309    }
310
311// ----------------------------------------------------
312    /**
313     * Internal class to handle return value.
314     *
315     * More of a struct, really
316     */
317    static class RetVal {
318
319        RetVal(int code, double x, double y, double z, double vs) {
320            this.code = code;
321            this.x = x;
322            this.y = y;
323            this.z = z;
324            this.vs = vs;
325        }
326        @SuppressFBWarnings(value = "URF_UNREAD_FIELD")
327        int code;
328        @SuppressFBWarnings(value = "URF_UNREAD_FIELD")
329        double x, y, z, vs;
330    }
331
332    private final static Logger log = LoggerFactory.getLogger(Analytic_AAlgorithm.class);
333
334}