1 /* 2 * Copyright (C) 2017 The Android Open Source Project 3 * 4 * Licensed under the Apache License, Version 2.0 (the "License"); 5 * you may not use this file except in compliance with the License. 6 * You may obtain a copy of the License at 7 * 8 * http://www.apache.org/licenses/LICENSE-2.0 9 * 10 * Unless required by applicable law or agreed to in writing, software 11 * distributed under the License is distributed on an "AS IS" BASIS, 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 * See the License for the specific language governing permissions and 14 * limitations under the License. 15 */ 16 17 package android.location.cts.pseudorange; 18 19 import static java.nio.charset.StandardCharsets.UTF_8; 20 21 import com.google.common.annotations.VisibleForTesting; 22 import com.google.common.base.Preconditions; 23 import com.google.common.collect.Lists; 24 import android.location.cts.pseudorange.Ecef2LlaConverter.GeodeticLlaValues; 25 import android.location.cts.pseudorange.EcefToTopocentricConverter.TopocentricAEDValues; 26 import android.location.cts.pseudorange.SatellitePositionCalculator.PositionAndVelocity; 27 import android.location.cts.nano.Ephemeris.GpsEphemerisProto; 28 import android.location.cts.nano.Ephemeris.GpsNavMessageProto; 29 import java.io.BufferedReader; 30 import java.io.IOException; 31 import java.io.InputStream; 32 import java.io.InputStreamReader; 33 import java.net.URI; 34 import java.util.ArrayList; 35 import java.util.Arrays; 36 import java.util.Collections; 37 import java.util.List; 38 import org.apache.commons.math.linear.Array2DRowRealMatrix; 39 import org.apache.commons.math.linear.LUDecompositionImpl; 40 import org.apache.commons.math.linear.QRDecompositionImpl; 41 import org.apache.commons.math.linear.RealMatrix; 42 43 /** 44 * Computes an iterative least square receiver position solution given the pseudorange (meters) and 45 * accumulated delta range (meters) measurements, receiver time of week, week number and the 46 * navigation message. 47 */ 48 class UserPositionVelocityWeightedLeastSquare { 49 private static final double SPEED_OF_LIGHT_MPS = 299792458.0; 50 private static final int SECONDS_IN_WEEK = 604800; 51 private static final double LEAST_SQUARE_TOLERANCE_METERS = 4.0e-8; 52 /** Position correction threshold below which atmospheric correction will be applied */ 53 private static final double ATMPOSPHERIC_CORRECTIONS_THRESHOLD_METERS = 1000.0; 54 private static final int MINIMUM_NUMER_OF_SATELLITES = 4; 55 private static final double RESIDUAL_TO_REPEAT_LEAST_SQUARE_METERS = 20.0; 56 57 private static final int MAXIMUM_NUMBER_OF_LEAST_SQUARE_ITERATIONS = 100; 58 /** Maximum possible number of GPS satellites */ 59 private static final int MAX_NUMBER_OF_SATELLITES = 32; 60 /** GPS C/A code chip width Tc = 1 microseconds */ 61 private static final double GPS_CHIP_WIDTH_T_C_SEC = 1.0e-6; 62 /** Narrow correlator with spacing d = 0.1 chip */ 63 private static final double GPS_CORRELATOR_SPACING_IN_CHIPS = 0.1; 64 /** Average time of DLL correlator T of 20 milliseconds */ 65 private static final double GPS_DLL_AVERAGING_TIME_SEC = 20.0e-3; 66 /** Average signal travel time from GPS satellite and earth */ 67 private static final double AVERAGE_TRAVEL_TIME_SECONDS = 70.0e-3; 68 private static final double SECONDS_PER_NANO = 1.0e-9; 69 private static final double DOUBLE_ROUND_OFF_TOLERANCE = 0.0000000001; 70 private PseudorangeSmoother pseudorangeSmoother = null; 71 private double geoidHeightMeters; 72 private boolean calculateGeoidMeters = true; 73 private RealMatrix geometryMatrix; 74 75 /** Default Constructor */ UserPositionVelocityWeightedLeastSquare()76 public UserPositionVelocityWeightedLeastSquare() { 77 } 78 79 /* 80 * Constructor with a smoother. One can implement their own smoothing algorithm for smoothing 81 * the pseudorange, by passing a class which implements {@link PseudorangeSmoother} interface. 82 */ UserPositionVelocityWeightedLeastSquare(PseudorangeSmoother pseudorangeSmoother)83 public UserPositionVelocityWeightedLeastSquare(PseudorangeSmoother pseudorangeSmoother) { 84 this.pseudorangeSmoother = pseudorangeSmoother; 85 } 86 87 /** 88 * Least square solution to calculate the user position given the navigation message, pseudorange 89 * and accumulated delta range measurements. Also calculates user velocity non-iteratively from 90 * Least square position solution. 91 * 92 * <p>The method fills the user position and velocity in ECEF coordinates and receiver clock 93 * offset in meters and clock offset rate in meters per second. 94 * 95 * <p>One can implement their own smoothing algorithm for smoothing the pseudorange, by passing 96 * a class which implements pseudorangeSmoother interface. 97 * 98 * <p>Source for least squares: 99 * <ul> 100 * <li>http://www.u-blox.com/images/downloads/Product_Docs/GPS_Compendium%28GPS-X-02007%29.pdf 101 * page 81 - 85 102 * <li>Parkinson, B.W., Spilker Jr., J.J.: ‘Global positioning system: theory and applications’ 103 * page 412 - 414 104 * </ul> 105 * 106 * @param navMessageProto parameters of the navigation message 107 * @param usefulSatellitesToReceiverMeasurements Map of useful satellite PRN to 108 * {@link GpsMeasurementWithRangeAndUncertainty} containing receiver measurements for 109 * computing the position solution. 110 * @param receiverGPSTowAtReceptionSeconds Receiver estimate of GPS time of week (seconds) 111 * @param receiverGPSWeek Receiver estimate of GPS week (0-1024+) 112 * @param dayOfYear1To366 The day of the year between 1 and 366 113 * @param positionVelocitySolutionECEF Solution array of the following format: 114 * [0-2] xyz solution of user. 115 * [3] clock bias of user. 116 * [4-6] velocity of user. 117 * [7] clock bias rate of user. 118 * @param positionVelocityUncertaintyEnu Uncertainty of calculated position and velocity solution 119 * in meters and mps local ENU system. Array has the following format: 120 * [0-2] Enu uncertainty of position solution in meters 121 * [3-5] Enu uncertainty of velocity solution in meters per second. 122 * 123 */ calculateUserPositionVelocityLeastSquare( GpsNavMessageProto navMessageProto, List<GpsMeasurementWithRangeAndUncertainty> usefulSatellitesToReceiverMeasurements, double receiverGPSTowAtReceptionSeconds, int receiverGPSWeek, int dayOfYear1To366, double[] positionVelocitySolutionECEF, double[] positionVelocityUncertaintyEnu)124 public void calculateUserPositionVelocityLeastSquare( 125 GpsNavMessageProto navMessageProto, 126 List<GpsMeasurementWithRangeAndUncertainty> usefulSatellitesToReceiverMeasurements, 127 double receiverGPSTowAtReceptionSeconds, 128 int receiverGPSWeek, 129 int dayOfYear1To366, 130 double[] positionVelocitySolutionECEF, 131 double[] positionVelocityUncertaintyEnu) 132 throws Exception { 133 134 double[] deltaPositionMeters; 135 // make a copy of usefulSatellitesToReceiverMeasurements, to keep the original list the same 136 List<GpsMeasurementWithRangeAndUncertainty> satellitesToReceiverMeasurements = 137 new ArrayList<GpsMeasurementWithRangeAndUncertainty>(usefulSatellitesToReceiverMeasurements); 138 if (pseudorangeSmoother != null) { 139 satellitesToReceiverMeasurements = 140 pseudorangeSmoother.updatePseudorangeSmoothingResult(satellitesToReceiverMeasurements); 141 } 142 int numberOfUsefulSatellites = 143 getNumberOfusefulSatellites(satellitesToReceiverMeasurements); 144 // Least square position solution is supported only if 4 or more satellites visible 145 Preconditions.checkArgument(numberOfUsefulSatellites >= MINIMUM_NUMER_OF_SATELLITES, 146 "At least 4 satellites have to be visible... Only 3D mode is supported..."); 147 boolean repeatLeastSquare = false; 148 SatellitesPositionPseudorangesResidualAndCovarianceMatrix satPosPseudorangeResidualAndWeight; 149 do { 150 // Calculate satellites' positions, measurement residual per visible satellite and weight 151 // matrix for the iterative least square 152 boolean doAtmosphericCorrections = false; 153 satPosPseudorangeResidualAndWeight = 154 calculateSatPosAndPseudorangeResidual( 155 navMessageProto, 156 satellitesToReceiverMeasurements, 157 receiverGPSTowAtReceptionSeconds, 158 receiverGPSWeek, 159 dayOfYear1To366, 160 positionVelocitySolutionECEF, 161 doAtmosphericCorrections); 162 163 // Calcualte the geometry matrix according to "Global Positioning System: Theory and 164 // Applications", Parkinson and Spilker page 413 165 RealMatrix covarianceMatrixM2 = 166 new Array2DRowRealMatrix(satPosPseudorangeResidualAndWeight.covarianceMatrixMetersSquare); 167 geometryMatrix = new Array2DRowRealMatrix(calculateGeometryMatrix( 168 satPosPseudorangeResidualAndWeight.satellitesPositionsMeters, 169 positionVelocitySolutionECEF)); 170 RealMatrix weightedGeometryMatrix; 171 RealMatrix weightMatrixMetersMinus2 = null; 172 // Apply weighted least square only if the covariance matrix is not singular (has a non-zero 173 // determinant), otherwise apply ordinary least square. The reason is to ignore reported 174 // signal to noise ratios by the receiver that can lead to such singularities 175 LUDecompositionImpl ludCovMatrixM2 = new LUDecompositionImpl(covarianceMatrixM2); 176 double det = ludCovMatrixM2.getDeterminant(); 177 178 if (det <= DOUBLE_ROUND_OFF_TOLERANCE) { 179 // Do not weight the geometry matrix if covariance matrix is singular. 180 weightedGeometryMatrix = geometryMatrix; 181 } else { 182 weightMatrixMetersMinus2 = ludCovMatrixM2.getSolver().getInverse(); 183 RealMatrix hMatrix = 184 calculateHMatrix(weightMatrixMetersMinus2, geometryMatrix); 185 weightedGeometryMatrix = hMatrix.multiply(geometryMatrix.transpose()) 186 .multiply(weightMatrixMetersMinus2); 187 } 188 189 // Equation 9 page 413 from "Global Positioning System: Theory and Applicaitons", Parkinson 190 // and Spilker 191 deltaPositionMeters = 192 GpsMathOperations.matrixByColVectMultiplication(weightedGeometryMatrix.getData(), 193 satPosPseudorangeResidualAndWeight.pseudorangeResidualsMeters); 194 195 // Apply corrections to the position estimate 196 positionVelocitySolutionECEF[0] += deltaPositionMeters[0]; 197 positionVelocitySolutionECEF[1] += deltaPositionMeters[1]; 198 positionVelocitySolutionECEF[2] += deltaPositionMeters[2]; 199 positionVelocitySolutionECEF[3] += deltaPositionMeters[3]; 200 // Iterate applying corrections to the position solution until correction is below threshold 201 satPosPseudorangeResidualAndWeight = 202 applyWeightedLeastSquare( 203 navMessageProto, 204 satellitesToReceiverMeasurements, 205 receiverGPSTowAtReceptionSeconds, 206 receiverGPSWeek, 207 dayOfYear1To366, 208 positionVelocitySolutionECEF, 209 deltaPositionMeters, 210 doAtmosphericCorrections, 211 satPosPseudorangeResidualAndWeight, 212 weightMatrixMetersMinus2); 213 repeatLeastSquare = false; 214 int satsWithResidualBelowThreshold = 215 satPosPseudorangeResidualAndWeight.pseudorangeResidualsMeters.length; 216 // remove satellites that have residuals above RESIDUAL_TO_REPEAT_LEAST_SQUARE_METERS as they 217 // worsen the position solution accuracy. If any satellite is removed, repeat the least square 218 repeatLeastSquare = 219 removeHighResidualSats( 220 satellitesToReceiverMeasurements, 221 repeatLeastSquare, 222 satPosPseudorangeResidualAndWeight, 223 satsWithResidualBelowThreshold); 224 225 } while (repeatLeastSquare); 226 calculateGeoidMeters = false; 227 228 // The computed ECEF position will be used next to compute the user velocity. 229 // we calculate and fill in the user velocity solutions based on following equation: 230 // Weight Matrix * GeometryMatrix * User Velocity Vector 231 // = Weight Matrix * deltaPseudoRangeRateWeightedMps 232 // Reference: Pratap Misra and Per Enge 233 // "Global Positioning System: Signals, Measurements, and Performance" Page 218. 234 235 // Gets the number of satellite used in Geometry Matrix 236 numberOfUsefulSatellites = geometryMatrix.getRowDimension(); 237 238 RealMatrix rangeRateMps = new Array2DRowRealMatrix(numberOfUsefulSatellites, 1); 239 RealMatrix deltaPseudoRangeRateMps = 240 new Array2DRowRealMatrix(numberOfUsefulSatellites, 1); 241 RealMatrix pseudorangeRateWeight 242 = new Array2DRowRealMatrix(numberOfUsefulSatellites, numberOfUsefulSatellites); 243 244 // Correct the receiver time of week with the estimated receiver clock bias 245 receiverGPSTowAtReceptionSeconds = 246 receiverGPSTowAtReceptionSeconds - positionVelocitySolutionECEF[3] / SPEED_OF_LIGHT_MPS; 247 248 int measurementCount = 0; 249 250 // Calculates range rates 251 for (int i = 0; i < MAX_NUMBER_OF_SATELLITES; i++) { 252 if (satellitesToReceiverMeasurements.get(i) != null) { 253 GpsEphemerisProto ephemeridesProto = getEphemerisForSatellite(navMessageProto, i + 1); 254 255 double pseudorangeMeasurementMeters = 256 satellitesToReceiverMeasurements.get(i).pseudorangeMeters; 257 GpsTimeOfWeekAndWeekNumber correctedTowAndWeek = 258 calculateCorrectedTransmitTowAndWeek(ephemeridesProto, receiverGPSTowAtReceptionSeconds, 259 receiverGPSWeek, pseudorangeMeasurementMeters); 260 261 // Calculate satellite velocity 262 PositionAndVelocity satPosECEFMetersVelocityMPS = SatellitePositionCalculator 263 .calculateSatellitePositionAndVelocityFromEphemeris( 264 ephemeridesProto, 265 correctedTowAndWeek.gpsTimeOfWeekSeconds, 266 correctedTowAndWeek.weekNumber, 267 positionVelocitySolutionECEF[0], 268 positionVelocitySolutionECEF[1], 269 positionVelocitySolutionECEF[2]); 270 271 // Calculates satellite clock error rate 272 double satelliteClockErrorRateMps = SatelliteClockCorrectionCalculator. 273 calculateSatClockCorrErrorRate( 274 ephemeridesProto, 275 correctedTowAndWeek.gpsTimeOfWeekSeconds, 276 correctedTowAndWeek.weekNumber); 277 278 // Fill in range rates. range rate = satellite velocity (dot product) line-of-sight vector 279 rangeRateMps.setEntry(measurementCount, 0, -1 * ( 280 satPosECEFMetersVelocityMPS.velocityXMetersPerSec 281 * geometryMatrix.getEntry(measurementCount, 0) 282 + satPosECEFMetersVelocityMPS.velocityYMetersPerSec 283 * geometryMatrix.getEntry(measurementCount, 1) 284 + satPosECEFMetersVelocityMPS.velocityZMetersPerSec 285 * geometryMatrix.getEntry(measurementCount, 2))); 286 287 deltaPseudoRangeRateMps.setEntry(measurementCount, 0, 288 satellitesToReceiverMeasurements.get(i).pseudorangeRateMps 289 - rangeRateMps.getEntry(measurementCount, 0) + satelliteClockErrorRateMps 290 - positionVelocitySolutionECEF[7]); 291 292 // Calculate the velocity weight matrix by using 1 / square(Pseudorangerate Uncertainty) 293 // along the diagonal 294 pseudorangeRateWeight.setEntry(measurementCount, measurementCount, 295 1 / (satellitesToReceiverMeasurements 296 .get(i).pseudorangeRateUncertaintyMps 297 * satellitesToReceiverMeasurements 298 .get(i).pseudorangeRateUncertaintyMps)); 299 measurementCount++; 300 } 301 } 302 303 RealMatrix weightedGeoMatrix = pseudorangeRateWeight.multiply(geometryMatrix); 304 RealMatrix deltaPseudoRangeRateWeightedMps = 305 pseudorangeRateWeight.multiply(deltaPseudoRangeRateMps); 306 QRDecompositionImpl qrdWeightedGeoMatrix = new QRDecompositionImpl(weightedGeoMatrix); 307 RealMatrix velocityMps 308 = qrdWeightedGeoMatrix.getSolver().solve(deltaPseudoRangeRateWeightedMps); 309 positionVelocitySolutionECEF[4] = velocityMps.getEntry(0, 0); 310 positionVelocitySolutionECEF[5] = velocityMps.getEntry(1, 0); 311 positionVelocitySolutionECEF[6] = velocityMps.getEntry(2, 0); 312 positionVelocitySolutionECEF[7] = velocityMps.getEntry(3, 0); 313 314 RealMatrix pseudorangeWeight 315 = new LUDecompositionImpl( 316 new Array2DRowRealMatrix(satPosPseudorangeResidualAndWeight.covarianceMatrixMetersSquare 317 ) 318 ).getSolver().getInverse(); 319 320 // Calculates and store the uncertainties of position and velocity in local ENU system in meters 321 // and meters per second. 322 double[] pvUncertainty = 323 calculatePositionVelocityUncertaintyEnu(pseudorangeRateWeight, pseudorangeWeight, 324 positionVelocitySolutionECEF); 325 System.arraycopy(pvUncertainty, 326 0 /*source starting pos*/, 327 positionVelocityUncertaintyEnu, 328 0 /*destination starting pos*/, 329 6 /*length of elements*/); 330 } 331 332 /** 333 * Calculates the position uncertainty in meters and the velocity uncertainty 334 * in meters per second solution in local ENU system. 335 * 336 * <p> Reference: Global Positioning System: Signals, Measurements, and Performance 337 * by Pratap Misra, Per Enge, Page 206 - 209. 338 * 339 * @param velocityWeightMatrix the velocity weight matrix 340 * @param positionWeightMatrix the position weight matrix 341 * @param positionVelocitySolution the position and velocity solution in ECEF 342 * @return an array containing the position and velocity uncertainties in ENU coordinate system. 343 * [0-2] Enu uncertainty of position solution in meters. 344 * [3-5] Enu uncertainty of velocity solution in meters per second. 345 */ calculatePositionVelocityUncertaintyEnu( RealMatrix velocityWeightMatrix, RealMatrix positionWeightMatrix, double[] positionVelocitySolution)346 public double[] calculatePositionVelocityUncertaintyEnu( 347 RealMatrix velocityWeightMatrix, RealMatrix positionWeightMatrix, 348 double[] positionVelocitySolution){ 349 350 if (geometryMatrix == null){ 351 return null; 352 } 353 354 RealMatrix velocityH = calculateHMatrix(velocityWeightMatrix, geometryMatrix); 355 RealMatrix positionH = calculateHMatrix(positionWeightMatrix, geometryMatrix); 356 357 // Calculate the rotation Matrix to convert to local ENU system. 358 RealMatrix rotationMatrix = new Array2DRowRealMatrix(4, 4); 359 GeodeticLlaValues llaValues = Ecef2LlaConverter.convertECEFToLLACloseForm 360 (positionVelocitySolution[0], positionVelocitySolution[1], positionVelocitySolution[2]); 361 rotationMatrix.setSubMatrix( 362 Ecef2EnuConverter.getRotationMatrix(llaValues.longitudeRadians, 363 llaValues.latitudeRadians).getData(), 0, 0); 364 rotationMatrix.setEntry(3, 3, 1); 365 366 // Convert to local ENU by pre-multiply rotation matrix and multiply rotation matrix transposed 367 velocityH = rotationMatrix.multiply(velocityH).multiply(rotationMatrix.transpose()); 368 positionH = rotationMatrix.multiply(positionH).multiply(rotationMatrix.transpose()); 369 370 // Return the square root of diagonal entries 371 return new double[] { 372 Math.sqrt(positionH.getEntry(0, 0)), Math.sqrt(positionH.getEntry(1, 1)), 373 Math.sqrt(positionH.getEntry(2, 2)), Math.sqrt(velocityH.getEntry(0, 0)), 374 Math.sqrt(velocityH.getEntry(1, 1)), Math.sqrt(velocityH.getEntry(2, 2))}; 375 } 376 377 /** 378 * Calculate the measurement connection matrix H as a function of weightMatrix and 379 * geometryMatrix. 380 * 381 * <p> H = (geometryMatrixTransposed * Weight * geometryMatrix) ^ -1 382 * 383 * <p> Reference: Global Positioning System: Signals, Measurements, and Performance, P207 384 * @param weightMatrix Weights for computing H Matrix 385 * @return H Matrix 386 */ calculateHMatrix(RealMatrix weightMatrix, RealMatrix geometryMatrix)387 private RealMatrix calculateHMatrix 388 (RealMatrix weightMatrix, RealMatrix geometryMatrix){ 389 390 RealMatrix tempH = geometryMatrix.transpose().multiply(weightMatrix).multiply(geometryMatrix); 391 return new LUDecompositionImpl(tempH).getSolver().getInverse(); 392 } 393 394 /** 395 * Applies weighted least square iterations and corrects to the position solution until correction 396 * is below threshold. An exception is thrown if the maximum number of iterations: 397 * {@value #MAXIMUM_NUMBER_OF_LEAST_SQUARE_ITERATIONS} is reached without convergence. 398 */ applyWeightedLeastSquare( GpsNavMessageProto navMessageProto, List<GpsMeasurementWithRangeAndUncertainty> usefulSatellitesToReceiverMeasurements, double receiverGPSTowAtReceptionSeconds, int receiverGPSWeek, int dayOfYear1To366, double[] positionSolutionECEF, double[] deltaPositionMeters, boolean doAtmosphericCorrections, SatellitesPositionPseudorangesResidualAndCovarianceMatrix satPosPseudorangeResidualAndWeight, RealMatrix weightMatrixMetersMinus2)399 private SatellitesPositionPseudorangesResidualAndCovarianceMatrix applyWeightedLeastSquare( 400 GpsNavMessageProto navMessageProto, 401 List<GpsMeasurementWithRangeAndUncertainty> usefulSatellitesToReceiverMeasurements, 402 double receiverGPSTowAtReceptionSeconds, 403 int receiverGPSWeek, 404 int dayOfYear1To366, 405 double[] positionSolutionECEF, 406 double[] deltaPositionMeters, 407 boolean doAtmosphericCorrections, 408 SatellitesPositionPseudorangesResidualAndCovarianceMatrix satPosPseudorangeResidualAndWeight, 409 RealMatrix weightMatrixMetersMinus2) 410 throws Exception { 411 RealMatrix weightedGeometryMatrix; 412 int numberOfIterations = 0; 413 414 while ((Math.abs(deltaPositionMeters[0]) + Math.abs(deltaPositionMeters[1]) 415 + Math.abs(deltaPositionMeters[2])) >= LEAST_SQUARE_TOLERANCE_METERS) { 416 // Apply ionospheric and tropospheric corrections only if the applied correction to 417 // position is below a specific threshold 418 if ((Math.abs(deltaPositionMeters[0]) + Math.abs(deltaPositionMeters[1]) 419 + Math.abs(deltaPositionMeters[2])) < ATMPOSPHERIC_CORRECTIONS_THRESHOLD_METERS) { 420 doAtmosphericCorrections = true; 421 } 422 // Calculate satellites' positions, measurement residual per visible satellite and weight 423 // matrix for the iterative least square 424 satPosPseudorangeResidualAndWeight = calculateSatPosAndPseudorangeResidual(navMessageProto, 425 usefulSatellitesToReceiverMeasurements, receiverGPSTowAtReceptionSeconds, receiverGPSWeek, 426 dayOfYear1To366, positionSolutionECEF, doAtmosphericCorrections); 427 428 // Calculate the geometry matrix according to "Global Positioning System: Theory and 429 // Applications", Parkinson and Spilker page 413 430 geometryMatrix = new Array2DRowRealMatrix(calculateGeometryMatrix( 431 satPosPseudorangeResidualAndWeight.satellitesPositionsMeters, positionSolutionECEF)); 432 // Apply weighted least square only if the covariance matrix is 433 // not singular (has a non-zero determinant), otherwise apply ordinary least square. 434 // The reason is to ignore reported signal to noise ratios by the receiver that can 435 // lead to such singularities 436 if (weightMatrixMetersMinus2 == null) { 437 weightedGeometryMatrix = geometryMatrix; 438 } else { 439 RealMatrix hMatrix = 440 calculateHMatrix(weightMatrixMetersMinus2, geometryMatrix); 441 weightedGeometryMatrix = hMatrix.multiply(geometryMatrix.transpose()) 442 .multiply(weightMatrixMetersMinus2); 443 } 444 445 // Equation 9 page 413 from "Global Positioning System: Theory and Applicaitons", 446 // Parkinson and Spilker 447 deltaPositionMeters = 448 GpsMathOperations.matrixByColVectMultiplication( 449 weightedGeometryMatrix.getData(), 450 satPosPseudorangeResidualAndWeight.pseudorangeResidualsMeters); 451 452 // Apply corrections to the position estimate 453 positionSolutionECEF[0] += deltaPositionMeters[0]; 454 positionSolutionECEF[1] += deltaPositionMeters[1]; 455 positionSolutionECEF[2] += deltaPositionMeters[2]; 456 positionSolutionECEF[3] += deltaPositionMeters[3]; 457 numberOfIterations++; 458 Preconditions.checkArgument(numberOfIterations <= MAXIMUM_NUMBER_OF_LEAST_SQUARE_ITERATIONS, 459 "Maximum number of least square iterations reached without convergance..."); 460 } 461 return satPosPseudorangeResidualAndWeight; 462 } 463 464 /** 465 * Removes satellites that have residuals above {@value #RESIDUAL_TO_REPEAT_LEAST_SQUARE_METERS} 466 * from the {@code usefulSatellitesToReceiverMeasurements} list. Returns true if any satellite is 467 * removed. 468 */ removeHighResidualSats( List<GpsMeasurementWithRangeAndUncertainty> usefulSatellitesToReceiverMeasurements, boolean repeatLeastSquare, SatellitesPositionPseudorangesResidualAndCovarianceMatrix satPosPseudorangeResidualAndWeight, int satsWithResidualBelowThreshold)469 private boolean removeHighResidualSats( 470 List<GpsMeasurementWithRangeAndUncertainty> usefulSatellitesToReceiverMeasurements, 471 boolean repeatLeastSquare, 472 SatellitesPositionPseudorangesResidualAndCovarianceMatrix satPosPseudorangeResidualAndWeight, 473 int satsWithResidualBelowThreshold) { 474 475 for (int i = 0; i < satPosPseudorangeResidualAndWeight.pseudorangeResidualsMeters.length; i++) { 476 if (satsWithResidualBelowThreshold > MINIMUM_NUMER_OF_SATELLITES) { 477 if (Math.abs(satPosPseudorangeResidualAndWeight.pseudorangeResidualsMeters[i]) 478 > RESIDUAL_TO_REPEAT_LEAST_SQUARE_METERS) { 479 int prn = satPosPseudorangeResidualAndWeight.satellitePRNs[i]; 480 usefulSatellitesToReceiverMeasurements.set(prn - 1, null); 481 satsWithResidualBelowThreshold--; 482 repeatLeastSquare = true; 483 } 484 } 485 } 486 return repeatLeastSquare; 487 } 488 489 /** 490 * Calculates position of all visible satellites and pseudorange measurement residual (difference 491 * of measured to predicted pseudoranges) needed for the least square computation. The result is 492 * stored in an instance of {@link SatellitesPositionPseudorangesResidualAndCovarianceMatrix} 493 * 494 * @param navMeassageProto parameters of the navigation message 495 * @param usefulSatellitesToReceiverMeasurements Map of useful satellite PRN to 496 * {@link GpsMeasurementWithRangeAndUncertainty} containing receiver measurements for 497 * computing the position solution 498 * @param receiverGPSTowAtReceptionSeconds Receiver estimate of GPS time of week (seconds) 499 * @param receiverGpsWeek Receiver estimate of GPS week (0-1024+) 500 * @param dayOfYear1To366 The day of the year between 1 and 366 501 * @param userPositionECEFMeters receiver ECEF position in meters 502 * @param doAtmosphericCorrections boolean indicating if atmospheric range corrections should be 503 * applied 504 * @return SatellitesPositionPseudorangesResidualAndCovarianceMatrix Object containing satellite 505 * prns, satellite positions in ECEF, pseudorange residuals and covariance matrix. 506 */ 507 public SatellitesPositionPseudorangesResidualAndCovarianceMatrix calculateSatPosAndPseudorangeResidual( GpsNavMessageProto navMeassageProto, List<GpsMeasurementWithRangeAndUncertainty> usefulSatellitesToReceiverMeasurements, double receiverGPSTowAtReceptionSeconds, int receiverGpsWeek, int dayOfYear1To366, double[] userPositionECEFMeters, boolean doAtmosphericCorrections)508 calculateSatPosAndPseudorangeResidual( 509 GpsNavMessageProto navMeassageProto, 510 List<GpsMeasurementWithRangeAndUncertainty> usefulSatellitesToReceiverMeasurements, 511 double receiverGPSTowAtReceptionSeconds, 512 int receiverGpsWeek, 513 int dayOfYear1To366, 514 double[] userPositionECEFMeters, 515 boolean doAtmosphericCorrections) 516 throws Exception { 517 int numberOfUsefulSatellites = 518 getNumberOfusefulSatellites(usefulSatellitesToReceiverMeasurements); 519 // deltaPseudorange is the pseudorange measurement residual 520 double[] deltaPseudorangesMeters = new double[numberOfUsefulSatellites]; 521 double[][] satellitesPositionsECEFMeters = new double[numberOfUsefulSatellites][3]; 522 523 // satellite PRNs 524 int[] satellitePRNs = new int[numberOfUsefulSatellites]; 525 526 // Ionospheric model parameters 527 double[] alpha = 528 {navMeassageProto.iono.alpha[0], navMeassageProto.iono.alpha[1], 529 navMeassageProto.iono.alpha[2], navMeassageProto.iono.alpha[3]}; 530 double[] beta = {navMeassageProto.iono.beta[0], navMeassageProto.iono.beta[1], 531 navMeassageProto.iono.beta[2], navMeassageProto.iono.beta[3]}; 532 // Weight matrix for the weighted least square 533 RealMatrix covarianceMatrixMetersSquare = 534 new Array2DRowRealMatrix(numberOfUsefulSatellites, numberOfUsefulSatellites); 535 calculateSatPosAndResiduals( 536 navMeassageProto, 537 usefulSatellitesToReceiverMeasurements, 538 receiverGPSTowAtReceptionSeconds, 539 receiverGpsWeek, 540 dayOfYear1To366, 541 userPositionECEFMeters, 542 doAtmosphericCorrections, 543 deltaPseudorangesMeters, 544 satellitesPositionsECEFMeters, 545 satellitePRNs, 546 alpha, 547 beta, 548 covarianceMatrixMetersSquare); 549 550 return new SatellitesPositionPseudorangesResidualAndCovarianceMatrix(satellitePRNs, 551 satellitesPositionsECEFMeters, deltaPseudorangesMeters, 552 covarianceMatrixMetersSquare.getData()); 553 } 554 555 /** 556 * Calculates and fill the position of all visible satellites: 557 * {@code satellitesPositionsECEFMeters}, pseudorange measurement residual (difference of measured 558 * to predicted pseudoranges): {@code deltaPseudorangesMeters} and covariance matrix from the 559 * weighted least square: {@code covarianceMatrixMetersSquare}. An array of the satellite PRNs 560 * {@code satellitePRNs} is as well filled. 561 */ calculateSatPosAndResiduals( GpsNavMessageProto navMeassageProto, List<GpsMeasurementWithRangeAndUncertainty> usefulSatellitesToReceiverMeasurements, double receiverGPSTowAtReceptionSeconds, int receiverGpsWeek, int dayOfYear1To366, double[] userPositionECEFMeters, boolean doAtmosphericCorrections, double[] deltaPseudorangesMeters, double[][] satellitesPositionsECEFMeters, int[] satellitePRNs, double[] alpha, double[] beta, RealMatrix covarianceMatrixMetersSquare)562 private void calculateSatPosAndResiduals( 563 GpsNavMessageProto navMeassageProto, 564 List<GpsMeasurementWithRangeAndUncertainty> usefulSatellitesToReceiverMeasurements, 565 double receiverGPSTowAtReceptionSeconds, 566 int receiverGpsWeek, 567 int dayOfYear1To366, 568 double[] userPositionECEFMeters, 569 boolean doAtmosphericCorrections, 570 double[] deltaPseudorangesMeters, 571 double[][] satellitesPositionsECEFMeters, 572 int[] satellitePRNs, 573 double[] alpha, 574 double[] beta, 575 RealMatrix covarianceMatrixMetersSquare) 576 throws Exception { 577 // user position without the clock estimate 578 double[] userPositionTempECEFMeters = 579 {userPositionECEFMeters[0], userPositionECEFMeters[1], userPositionECEFMeters[2]}; 580 int satsCounter = 0; 581 for (int i = 0; i < MAX_NUMBER_OF_SATELLITES; i++) { 582 if (usefulSatellitesToReceiverMeasurements.get(i) != null) { 583 GpsEphemerisProto ephemeridesProto = getEphemerisForSatellite(navMeassageProto, i + 1); 584 // Correct the receiver time of week with the estimated receiver clock bias 585 receiverGPSTowAtReceptionSeconds = 586 receiverGPSTowAtReceptionSeconds - userPositionECEFMeters[3] / SPEED_OF_LIGHT_MPS; 587 588 double pseudorangeMeasurementMeters = 589 usefulSatellitesToReceiverMeasurements.get(i).pseudorangeMeters; 590 double pseudorangeUncertaintyMeters = 591 usefulSatellitesToReceiverMeasurements.get(i).pseudorangeUncertaintyMeters; 592 593 // Assuming uncorrelated pseudorange measurements, the covariance matrix will be diagonal as 594 // follows 595 covarianceMatrixMetersSquare.setEntry(satsCounter, satsCounter, 596 pseudorangeUncertaintyMeters * pseudorangeUncertaintyMeters); 597 598 // Calculate time of week at transmission time corrected with the satellite clock drift 599 GpsTimeOfWeekAndWeekNumber correctedTowAndWeek = 600 calculateCorrectedTransmitTowAndWeek(ephemeridesProto, receiverGPSTowAtReceptionSeconds, 601 receiverGpsWeek, pseudorangeMeasurementMeters); 602 603 // calculate satellite position and velocity 604 PositionAndVelocity satPosECEFMetersVelocityMPS = SatellitePositionCalculator 605 .calculateSatellitePositionAndVelocityFromEphemeris(ephemeridesProto, 606 correctedTowAndWeek.gpsTimeOfWeekSeconds, correctedTowAndWeek.weekNumber, 607 userPositionECEFMeters[0], userPositionECEFMeters[1], userPositionECEFMeters[2]); 608 609 satellitesPositionsECEFMeters[satsCounter][0] = satPosECEFMetersVelocityMPS.positionXMeters; 610 satellitesPositionsECEFMeters[satsCounter][1] = satPosECEFMetersVelocityMPS.positionYMeters; 611 satellitesPositionsECEFMeters[satsCounter][2] = satPosECEFMetersVelocityMPS.positionZMeters; 612 613 // Calculate ionospheric and tropospheric corrections 614 double ionosphericCorrectionMeters; 615 double troposphericCorrectionMeters; 616 if (doAtmosphericCorrections) { 617 ionosphericCorrectionMeters = 618 IonosphericModel.ionoKloboucharCorrectionSeconds( 619 userPositionTempECEFMeters, 620 satellitesPositionsECEFMeters[satsCounter], 621 correctedTowAndWeek.gpsTimeOfWeekSeconds, 622 alpha, 623 beta, 624 IonosphericModel.L1_FREQ_HZ) 625 * SPEED_OF_LIGHT_MPS; 626 627 troposphericCorrectionMeters = 628 calculateTroposphericCorrectionMeters( 629 dayOfYear1To366, 630 satellitesPositionsECEFMeters, 631 userPositionTempECEFMeters, 632 satsCounter); 633 } else { 634 troposphericCorrectionMeters = 0.0; 635 ionosphericCorrectionMeters = 0.0; 636 } 637 double predictedPseudorangeMeters = 638 calculatePredictedPseudorange(userPositionECEFMeters, satellitesPositionsECEFMeters, 639 userPositionTempECEFMeters, satsCounter, ephemeridesProto, correctedTowAndWeek, 640 ionosphericCorrectionMeters, troposphericCorrectionMeters); 641 642 // Pseudorange residual (difference of measured to predicted pseudoranges) 643 deltaPseudorangesMeters[satsCounter] = 644 pseudorangeMeasurementMeters - predictedPseudorangeMeters; 645 646 // Satellite PRNs 647 satellitePRNs[satsCounter] = i + 1; 648 satsCounter++; 649 } 650 } 651 } 652 653 /** Searches ephemerides list for the ephemeris associated with current satellite in process */ getEphemerisForSatellite(GpsNavMessageProto navMeassageProto, int satPrn)654 private GpsEphemerisProto getEphemerisForSatellite(GpsNavMessageProto navMeassageProto, 655 int satPrn) { 656 List<GpsEphemerisProto> ephemeridesList 657 = new ArrayList<GpsEphemerisProto>(Arrays.asList(navMeassageProto.ephemerids)); 658 GpsEphemerisProto ephemeridesProto = null; 659 int ephemerisPrn = 0; 660 for (GpsEphemerisProto ephProtoFromList : ephemeridesList) { 661 ephemerisPrn = ephProtoFromList.prn; 662 if (ephemerisPrn == satPrn) { 663 ephemeridesProto = ephProtoFromList; 664 break; 665 } 666 } 667 return ephemeridesProto; 668 } 669 670 /** Calculates predicted pseudorange in meters */ calculatePredictedPseudorange(double[] userPositionECEFMeters, double[][] satellitesPositionsECEFMeters, double[] userPositionNoClockECEFMeters, int satsCounter, GpsEphemerisProto ephemeridesProto, GpsTimeOfWeekAndWeekNumber correctedTowAndWeek, double ionosphericCorrectionMeters, double troposphericCorrectionMeters)671 private double calculatePredictedPseudorange(double[] userPositionECEFMeters, 672 double[][] satellitesPositionsECEFMeters, double[] userPositionNoClockECEFMeters, 673 int satsCounter, GpsEphemerisProto ephemeridesProto, 674 GpsTimeOfWeekAndWeekNumber correctedTowAndWeek, double ionosphericCorrectionMeters, 675 double troposphericCorrectionMeters) throws Exception { 676 // Calcualte the satellite clock drift 677 double satelliteClockCorrectionMeters = 678 SatelliteClockCorrectionCalculator.calculateSatClockCorrAndEccAnomAndTkIteratively( 679 ephemeridesProto, correctedTowAndWeek.gpsTimeOfWeekSeconds, 680 correctedTowAndWeek.weekNumber).satelliteClockCorrectionMeters; 681 682 double satelliteToUserDistanceMeters = 683 GpsMathOperations.vectorNorm(GpsMathOperations.subtractTwoVectors( 684 satellitesPositionsECEFMeters[satsCounter], userPositionNoClockECEFMeters)); 685 686 // Predicted pseudorange 687 double predictedPseudorangeMeters = 688 satelliteToUserDistanceMeters - satelliteClockCorrectionMeters + ionosphericCorrectionMeters 689 + troposphericCorrectionMeters + userPositionECEFMeters[3]; 690 return predictedPseudorangeMeters; 691 } 692 693 /** Calculates the Gps troposheric correction in meters */ calculateTroposphericCorrectionMeters(int dayOfYear1To366, double[][] satellitesPositionsECEFMeters, double[] userPositionTempECEFMeters, int satsCounter)694 private double calculateTroposphericCorrectionMeters(int dayOfYear1To366, 695 double[][] satellitesPositionsECEFMeters, double[] userPositionTempECEFMeters, 696 int satsCounter) { 697 double troposphericCorrectionMeters; 698 TopocentricAEDValues elevationAzimuthDist = 699 EcefToTopocentricConverter.convertCartesianToTopocentericRadMeters( 700 userPositionTempECEFMeters, GpsMathOperations.subtractTwoVectors( 701 satellitesPositionsECEFMeters[satsCounter], userPositionTempECEFMeters)); 702 703 GeodeticLlaValues lla = 704 Ecef2LlaConverter.convertECEFToLLACloseForm(userPositionTempECEFMeters[0], 705 userPositionTempECEFMeters[1], userPositionTempECEFMeters[2]); 706 707 double elevationMetersAboveSeaLevel = 0.0; 708 if (calculateGeoidMeters) { 709 geoidHeightMeters = lla.altitudeMeters; 710 troposphericCorrectionMeters = TroposphericModelEgnos.calculateTropoCorrectionMeters( 711 elevationAzimuthDist.elevationRadians, lla.latitudeRadians, elevationMetersAboveSeaLevel, 712 dayOfYear1To366); 713 } else { 714 troposphericCorrectionMeters = TroposphericModelEgnos.calculateTropoCorrectionMeters( 715 elevationAzimuthDist.elevationRadians, lla.latitudeRadians, 716 lla.altitudeMeters - geoidHeightMeters, dayOfYear1To366); 717 } 718 return troposphericCorrectionMeters; 719 } 720 721 /** 722 * Gets the number of useful satellites from a list of 723 * {@link GpsMeasurementWithRangeAndUncertainty}. 724 */ getNumberOfusefulSatellites( List<GpsMeasurementWithRangeAndUncertainty> usefulSatellitesToReceiverMeasurements)725 private int getNumberOfusefulSatellites( 726 List<GpsMeasurementWithRangeAndUncertainty> usefulSatellitesToReceiverMeasurements) { 727 // calculate the number of useful satellites 728 int numberOfUsefulSatellites = 0; 729 for (int i = 0; i < usefulSatellitesToReceiverMeasurements.size(); i++) { 730 if (usefulSatellitesToReceiverMeasurements.get(i) != null) { 731 numberOfUsefulSatellites++; 732 } 733 } 734 return numberOfUsefulSatellites; 735 } 736 737 /** 738 * Computes the GPS time of week at the time of transmission and as well the corrected GPS week 739 * taking into consideration week rollover. The returned GPS time of week is corrected by the 740 * computed satellite clock drift. The result is stored in an instance of 741 * {@link GpsTimeOfWeekAndWeekNumber} 742 * 743 * @param ephemerisProto parameters of the navigation message 744 * @param receiverGpsTowAtReceptionSeconds Receiver estimate of GPS time of week when signal was 745 * received (seconds) 746 * @param receiverGpsWeek Receiver estimate of GPS week (0-1024+) 747 * @param pseudorangeMeters Measured pseudorange in meters 748 * @return GpsTimeOfWeekAndWeekNumber Object containing Gps time of week and week number. 749 */ calculateCorrectedTransmitTowAndWeek( GpsEphemerisProto ephemerisProto, double receiverGpsTowAtReceptionSeconds, int receiverGpsWeek, double pseudorangeMeters)750 private static GpsTimeOfWeekAndWeekNumber calculateCorrectedTransmitTowAndWeek( 751 GpsEphemerisProto ephemerisProto, double receiverGpsTowAtReceptionSeconds, 752 int receiverGpsWeek, double pseudorangeMeters) throws Exception { 753 // GPS time of week at time of transmission: Gps time corrected for transit time (page 98 ICD 754 // GPS 200) 755 double receiverGpsTowAtTimeOfTransmission = 756 receiverGpsTowAtReceptionSeconds - pseudorangeMeters / SPEED_OF_LIGHT_MPS; 757 758 // Adjust for week rollover 759 if (receiverGpsTowAtTimeOfTransmission < 0) { 760 receiverGpsTowAtTimeOfTransmission += SECONDS_IN_WEEK; 761 receiverGpsWeek -= 1; 762 } else if (receiverGpsTowAtTimeOfTransmission > SECONDS_IN_WEEK) { 763 receiverGpsTowAtTimeOfTransmission -= SECONDS_IN_WEEK; 764 receiverGpsWeek += 1; 765 } 766 767 // Compute the satellite clock correction term (Seconds) 768 double clockCorrectionSeconds = 769 SatelliteClockCorrectionCalculator.calculateSatClockCorrAndEccAnomAndTkIteratively( 770 ephemerisProto, receiverGpsTowAtTimeOfTransmission, 771 receiverGpsWeek).satelliteClockCorrectionMeters / SPEED_OF_LIGHT_MPS; 772 773 // Correct with the satellite clock correction term 774 double receiverGpsTowAtTimeOfTransmissionCorrectedSec = 775 receiverGpsTowAtTimeOfTransmission + clockCorrectionSeconds; 776 777 // Adjust for week rollover due to satellite clock correction 778 if (receiverGpsTowAtTimeOfTransmissionCorrectedSec < 0.0) { 779 receiverGpsTowAtTimeOfTransmissionCorrectedSec += SECONDS_IN_WEEK; 780 receiverGpsWeek -= 1; 781 } 782 if (receiverGpsTowAtTimeOfTransmissionCorrectedSec > SECONDS_IN_WEEK) { 783 receiverGpsTowAtTimeOfTransmissionCorrectedSec -= SECONDS_IN_WEEK; 784 receiverGpsWeek += 1; 785 } 786 return new GpsTimeOfWeekAndWeekNumber(receiverGpsTowAtTimeOfTransmissionCorrectedSec, 787 receiverGpsWeek); 788 } 789 790 /** 791 * Calculates the Geometry matrix (describing user to satellite geometry) given a list of 792 * satellite positions in ECEF coordinates in meters and the user position in ECEF in meters. 793 * 794 * <p>The geometry matrix has four columns, and rows equal to the number of satellites. For each 795 * of the rows (i.e. for each of the satellites used), the columns are filled with the normalized 796 * line–of-sight vectors and 1 s for the fourth column. 797 * 798 * <p>Source: Parkinson, B.W., Spilker Jr., J.J.: ‘Global positioning system: theory and 799 * applications’ page 413 800 */ calculateGeometryMatrix(double[][] satellitePositionsECEFMeters, double[] userPositionECEFMeters)801 private static double[][] calculateGeometryMatrix(double[][] satellitePositionsECEFMeters, 802 double[] userPositionECEFMeters) { 803 804 double[][] geometeryMatrix = new double[satellitePositionsECEFMeters.length][4]; 805 for (int i = 0; i < satellitePositionsECEFMeters.length; i++) { 806 geometeryMatrix[i][3] = 1; 807 } 808 // iterate over all satellites 809 for (int i = 0; i < satellitePositionsECEFMeters.length; i++) { 810 double[] r = {satellitePositionsECEFMeters[i][0] - userPositionECEFMeters[0], 811 satellitePositionsECEFMeters[i][1] - userPositionECEFMeters[1], 812 satellitePositionsECEFMeters[i][2] - userPositionECEFMeters[2]}; 813 double norm = Math.sqrt(Math.pow(r[0], 2) + Math.pow(r[1], 2) + Math.pow(r[2], 2)); 814 for (int j = 0; j < 3; j++) { 815 geometeryMatrix[i][j] = 816 (userPositionECEFMeters[j] - satellitePositionsECEFMeters[i][j]) / norm; 817 } 818 } 819 return geometeryMatrix; 820 } 821 822 /** 823 * Class containing satellites' PRNs, satellites' positions in ECEF meters, the peseudorange 824 * residual per visible satellite in meters and the covariance matrix of the pseudoranges in 825 * meters square 826 */ 827 private static class SatellitesPositionPseudorangesResidualAndCovarianceMatrix { 828 829 /** Satellites' PRNs */ 830 private final int[] satellitePRNs; 831 832 /** ECEF positions (meters) of useful satellites */ 833 private final double[][] satellitesPositionsMeters; 834 835 /** Pseudorange measurement residuals (difference of measured to predicted pseudoranges) */ 836 private final double[] pseudorangeResidualsMeters; 837 838 /** Pseudorange covariance Matrix for the weighted least squares (meters square) */ 839 private final double[][] covarianceMatrixMetersSquare; 840 841 /** Constructor */ SatellitesPositionPseudorangesResidualAndCovarianceMatrix(int[] satellitePRNs, double[][] satellitesPositionsMeters, double[] pseudorangeResidualsMeters, double[][] covarianceMatrixMetersSquare)842 private SatellitesPositionPseudorangesResidualAndCovarianceMatrix(int[] satellitePRNs, 843 double[][] satellitesPositionsMeters, double[] pseudorangeResidualsMeters, 844 double[][] covarianceMatrixMetersSquare) { 845 this.satellitePRNs = satellitePRNs; 846 this.satellitesPositionsMeters = satellitesPositionsMeters; 847 this.pseudorangeResidualsMeters = pseudorangeResidualsMeters; 848 this.covarianceMatrixMetersSquare = covarianceMatrixMetersSquare; 849 } 850 851 } 852 853 /** 854 * Class containing GPS time of week in seconds and GPS week number 855 */ 856 private static class GpsTimeOfWeekAndWeekNumber { 857 /** GPS time of week in seconds */ 858 private final double gpsTimeOfWeekSeconds; 859 860 /** GPS week number */ 861 private final int weekNumber; 862 863 /** Constructor */ GpsTimeOfWeekAndWeekNumber(double gpsTimeOfWeekSeconds, int weekNumber)864 private GpsTimeOfWeekAndWeekNumber(double gpsTimeOfWeekSeconds, int weekNumber) { 865 this.gpsTimeOfWeekSeconds = gpsTimeOfWeekSeconds; 866 this.weekNumber = weekNumber; 867 } 868 } 869 870 /** 871 * Uses the common reception time approach to calculate pseudoranges from the time of week 872 * measurements reported by the receiver according to http://cdn.intechopen.com/pdfs-wm/27712.pdf. 873 * As well computes the pseudoranges uncertainties for each input satellite 874 */ computePseudorangeAndUncertainties( List<GpsMeasurement> usefulSatellitesToReceiverMeasurements, Long[] usefulSatellitesToTOWNs, long largestTowNs)875 static List<GpsMeasurementWithRangeAndUncertainty> computePseudorangeAndUncertainties( 876 List<GpsMeasurement> usefulSatellitesToReceiverMeasurements, 877 Long[] usefulSatellitesToTOWNs, 878 long largestTowNs) { 879 880 List<GpsMeasurementWithRangeAndUncertainty> usefulSatellitesToPseudorangeMeasurements = 881 Arrays.asList( 882 new GpsMeasurementWithRangeAndUncertainty[MAX_NUMBER_OF_SATELLITES]); 883 for (int i = 0; i < MAX_NUMBER_OF_SATELLITES; i++) { 884 if (usefulSatellitesToTOWNs[i] != null) { 885 double deltai = largestTowNs - usefulSatellitesToTOWNs[i]; 886 double pseudorangeMeters = 887 (AVERAGE_TRAVEL_TIME_SECONDS + deltai * SECONDS_PER_NANO) * SPEED_OF_LIGHT_MPS; 888 889 double signalToNoiseRatioLinear = 890 Math.pow(10, usefulSatellitesToReceiverMeasurements.get(i).signalToNoiseRatioDb / 10.0); 891 // From Global Positoning System book, Misra and Enge, page 416, the uncertainty of the 892 // pseudorange measurement is calculated next. 893 // For GPS C/A code chip width Tc = 1 microseconds. Narrow correlator with spacing d = 0.1 894 // chip and an average time of DLL correlator T of 20 milliseconds are used. 895 double sigmaMeters = 896 SPEED_OF_LIGHT_MPS 897 * GPS_CHIP_WIDTH_T_C_SEC 898 * Math.sqrt( 899 GPS_CORRELATOR_SPACING_IN_CHIPS 900 / (4 * GPS_DLL_AVERAGING_TIME_SEC * signalToNoiseRatioLinear)); 901 usefulSatellitesToPseudorangeMeasurements.set( 902 i, 903 new GpsMeasurementWithRangeAndUncertainty( 904 usefulSatellitesToReceiverMeasurements.get(i), pseudorangeMeters, sigmaMeters)); 905 } 906 } 907 return usefulSatellitesToPseudorangeMeasurements; 908 } 909 910 } 911