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}