UserPositionVelocityWeightedLeastSquare

2017-10-24 6:51:50 来源: Gnsser 发布人:

/*

 * Copyright (C) 2017 The Android Open Source Project

 *

 * Licensed under the Apache License, Version 2.0 (the "License");

 * you may not use this file except in compliance with the License.

 * You may obtain a copy of the License at

 *

 *      http://www.apache.org/licenses/LICENSE-2.0

 *

 * Unless required by applicable law or agreed to in writing, software

 * distributed under the License is distributed on an "AS IS" BASIS,

 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.

 * See the License for the specific language governing permissions and

 * limitations under the License.

 */


package com.gnsser.android.pseudorange;


import com.google.common.annotations.VisibleForTesting;

import com.google.common.base.Preconditions;

import com.google.common.collect.Lists;

import com.gnsser.android.pseudorange.Ecef2LlaConverter.GeodeticLlaValues;

import com.gnsser.android.pseudorange.EcefToTopocentricConverter.TopocentricAEDValues;

import com.gnsser.android.pseudorange.SatellitePositionCalculator.PositionAndVelocity;

import android.location.cts.nano.Ephemeris.GpsEphemerisProto;

import android.location.cts.nano.Ephemeris.GpsNavMessageProto;


import java.util.ArrayList;

import java.util.Arrays;

import java.util.Collections;

import java.util.List;

import org.apache.commons.math3.linear.Array2DRowRealMatrix;

import org.apache.commons.math3.linear.LUDecomposition;

import org.apache.commons.math3.linear.QRDecomposition;

import org.apache.commons.math3.linear.RealMatrix;


/**

 * Computes an iterative least square receiver position solution given the pseudorange (meters) and

 * accumulated delta range (meters) measurements, receiver time of week, week number and the

 * navigation message.

 */

class UserPositionVelocityWeightedLeastSquare {

  private static final double SPEED_OF_LIGHT_MPS = 299792458.0;

  private static final int SECONDS_IN_WEEK = 604800;

  private static final double LEAST_SQUARE_TOLERANCE_METERS = 4.0e-8;

  /** Position correction threshold below which atmospheric correction will be applied */

  private static final double ATMPOSPHERIC_CORRECTIONS_THRESHOLD_METERS = 1000.0;

  private static final int MINIMUM_NUMER_OF_SATELLITES = 4;

  private static final double RESIDUAL_TO_REPEAT_LEAST_SQUARE_METERS = 20.0;

  private static final int MAXIMUM_NUMBER_OF_LEAST_SQUARE_ITERATIONS = 100;

  /** GPS C/A code chip width Tc = 1 microseconds */

  private static final double GPS_CHIP_WIDTH_T_C_SEC = 1.0e-6;

  /** Narrow correlator with spacing d = 0.1 chip */

  private static final double GPS_CORRELATOR_SPACING_IN_CHIPS = 0.1;

  /** Average time of DLL correlator T of 20 milliseconds */

  private static final double GPS_DLL_AVERAGING_TIME_SEC = 20.0e-3;

  /** Average signal travel time from GPS satellite and earth */

  private static final double AVERAGE_TRAVEL_TIME_SECONDS = 70.0e-3;

  private static final double SECONDS_PER_NANO = 1.0e-9;

  private static final double DOUBLE_ROUND_OFF_TOLERANCE = 0.0000000001;


  private final PseudorangeSmoother pseudorangeSmoother;

  private double geoidHeightMeters;

  private ElevationApiHelper elevationApiHelper;

  private boolean calculateGeoidMeters = true;

  private RealMatrix geometryMatrix;

  private double[] truthLocationForCorrectedResidualComputationEcef = null;


  /** Constructor */

  public UserPositionVelocityWeightedLeastSquare(PseudorangeSmoother pseudorangeSmoother) {

    this.pseudorangeSmoother = pseudorangeSmoother;

  }


  /** Constructor with Google Elevation API Key */

  public UserPositionVelocityWeightedLeastSquare(PseudorangeSmoother pseudorangeSmoother,

      String elevationApiKey){

    this.pseudorangeSmoother = pseudorangeSmoother;

    this.elevationApiHelper = new ElevationApiHelper(elevationApiKey);

  }


  /**

   * Sets the reference ground truth for pseudornage residual correction calculation. If no ground

   * truth is set, no corrected pesudorange residual will be calculated.

   */

  public void setTruthLocationForCorrectedResidualComputationEcef

  (double[] groundTruthForResidualCorrectionEcef) {

    this.truthLocationForCorrectedResidualComputationEcef = groundTruthForResidualCorrectionEcef;

  }


  /**

   * Least square solution to calculate the user position given the navigation message, pseudorange

   * and accumulated delta range measurements. Also calculates user velocity non-iteratively from

   * Least square position solution.

   *

   * <p>The method fills the user position and velocity in ECEF coordinates and receiver clock

   * offset in meters and clock offset rate in meters per second.

   *

   * <p>One can choose between no smoothing, using the carrier phase measurements (accumulated delta

   * range) or the doppler measurements (pseudorange rate) for smoothing the pseudorange. The

   * smoothing is applied only if time has changed below a specific threshold since last invocation.

   *

   * <p>Source for least squares:

   *

   * <ul>

   *   <li>http://www.u-blox.com/images/downloads/Product_Docs/GPS_Compendium%28GPS-X-02007%29.pdf

   *       page 81 - 85

   *   <li>Parkinson, B.W., Spilker Jr., J.J.: ‘Global positioning system: theory and applications’

   *       page 412 - 414

   * </ul>

   *

   * <p>Sources for smoothing pseudorange with carrier phase measurements:

   *

   * <ul>

   *   <li>Satellite Communications and Navigation Systems book, page 424,

   *   <li>Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems, page 388,

   *       389.

   * </ul>

   *

   * <p>The function does not modify the smoothed measurement list {@code

   * immutableSmoothedSatellitesToReceiverMeasurements}

   *

   * @param navMessageProto parameters of the navigation message

   * @param usefulSatellitesToReceiverMeasurements Map of useful satellite PRN to {@link

   *     GpsMeasurementWithRangeAndUncertainty} containing receiver measurements for computing the

   *     position solution.

   * @param receiverGPSTowAtReceptionSeconds Receiver estimate of GPS time of week (seconds)

   * @param receiverGPSWeek Receiver estimate of GPS week (0-1024+)

   * @param dayOfYear1To366 The day of the year between 1 and 366

   * @param positionVelocitySolutionECEF Solution array of the following format:

   *        [0-2] xyz solution of user.

   *        [3] clock bias of user.

   *        [4-6] velocity of user.

   *        [7] clock bias rate of user.

   * @param positionVelocityUncertaintyEnu Uncertainty of calculated position and velocity solution

   *     in meters and mps local ENU system. Array has the following format:

   *     [0-2] Enu uncertainty of position solution in meters

   *     [3-5] Enu uncertainty of velocity solution in meters per second.

   * @param pseudorangeResidualMeters The pseudorange residual corrected by subtracting expected

   *     psudorange calculated with the use clock bias of the highest elevation satellites.

   */

  public void calculateUserPositionVelocityLeastSquare(

      GpsNavMessageProto navMessageProto,

      List<GpsMeasurementWithRangeAndUncertainty> usefulSatellitesToReceiverMeasurements,

      double receiverGPSTowAtReceptionSeconds,

      int receiverGPSWeek,

      int dayOfYear1To366,

      double[] positionVelocitySolutionECEF,

      double[] positionVelocityUncertaintyEnu,

      double[] pseudorangeResidualMeters)

      throws Exception {


    // Use PseudorangeSmoother to smooth the pseudorange according to: Satellite Communications and

    // Navigation Systems book, page 424 and Principles of GNSS, Inertial, and Multisensor

    // Integrated Navigation Systems, page 388, 389.

    double[] deltaPositionMeters;

    List<GpsMeasurementWithRangeAndUncertainty> immutableSmoothedSatellitesToReceiverMeasurements =

        pseudorangeSmoother.updatePseudorangeSmoothingResult(

            Collections.unmodifiableList(usefulSatellitesToReceiverMeasurements));

    List<GpsMeasurementWithRangeAndUncertainty> mutableSmoothedSatellitesToReceiverMeasurements =

        Lists.newArrayList(immutableSmoothedSatellitesToReceiverMeasurements);

    int numberOfUsefulSatellites =

        getNumberOfUsefulSatellites(mutableSmoothedSatellitesToReceiverMeasurements);

    // Least square position solution is supported only if 4 or more satellites visible

    Preconditions.checkArgument(numberOfUsefulSatellites >= MINIMUM_NUMER_OF_SATELLITES,

        "At least 4 satellites have to be visible... Only 3D mode is supported...");

    boolean repeatLeastSquare = false;

    SatellitesPositionPseudorangesResidualAndCovarianceMatrix satPosPseudorangeResidualAndWeight;


    boolean isFirstWLS = true;


    do {

      // Calculate satellites' positions, measurement residuals per visible satellite and

      // weight matrix for the iterative least square

      boolean doAtmosphericCorrections = false;

      satPosPseudorangeResidualAndWeight =

          calculateSatPosAndPseudorangeResidual(

              navMessageProto,

              mutableSmoothedSatellitesToReceiverMeasurements,

              receiverGPSTowAtReceptionSeconds,

              receiverGPSWeek,

              dayOfYear1To366,

              positionVelocitySolutionECEF,

              doAtmosphericCorrections);


      // Calculate the geometry matrix according to "Global Positioning System: Theory and

      // Applications", Parkinson and Spilker page 413

      RealMatrix covarianceMatrixM2 =

          new Array2DRowRealMatrix(satPosPseudorangeResidualAndWeight.covarianceMatrixMetersSquare);

      geometryMatrix = new Array2DRowRealMatrix(calculateGeometryMatrix(

          satPosPseudorangeResidualAndWeight.satellitesPositionsMeters,

          positionVelocitySolutionECEF));

      RealMatrix weightedGeometryMatrix;

      RealMatrix weightMatrixMetersMinus2 = null;

      // Apply weighted least square only if the covariance matrix is not singular (has a non-zero

      // determinant), otherwise apply ordinary least square. The reason is to ignore reported

      // signal to noise ratios by the receiver that can lead to such singularities

      LUDecomposition ludCovMatrixM2 = new LUDecomposition(covarianceMatrixM2);

      double det = ludCovMatrixM2.getDeterminant();


      if (det <= DOUBLE_ROUND_OFF_TOLERANCE) {

        // Do not weight the geometry matrix if covariance matrix is singular.

        weightedGeometryMatrix = geometryMatrix;

      } else {

        weightMatrixMetersMinus2 = ludCovMatrixM2.getSolver().getInverse();

        RealMatrix hMatrix =

            calculateHMatrix(weightMatrixMetersMinus2, geometryMatrix);

        weightedGeometryMatrix = hMatrix.multiply(geometryMatrix.transpose())

            .multiply(weightMatrixMetersMinus2);

      }


      // Equation 9 page 413 from "Global Positioning System: Theory and Applicaitons", Parkinson

      // and Spilker

      deltaPositionMeters =

          GpsMathOperations.matrixByColVectMultiplication(weightedGeometryMatrix.getData(),

          satPosPseudorangeResidualAndWeight.pseudorangeResidualsMeters);


      // Apply corrections to the position estimate

      positionVelocitySolutionECEF[0] += deltaPositionMeters[0];

      positionVelocitySolutionECEF[1] += deltaPositionMeters[1];

      positionVelocitySolutionECEF[2] += deltaPositionMeters[2];

      positionVelocitySolutionECEF[3] += deltaPositionMeters[3];

      // Iterate applying corrections to the position solution until correction is below threshold

      satPosPseudorangeResidualAndWeight =

          applyWeightedLeastSquare(

              navMessageProto,

              mutableSmoothedSatellitesToReceiverMeasurements,

              receiverGPSTowAtReceptionSeconds,

              receiverGPSWeek,

              dayOfYear1To366,

              positionVelocitySolutionECEF,

              deltaPositionMeters,

              doAtmosphericCorrections,

              satPosPseudorangeResidualAndWeight,

              weightMatrixMetersMinus2);


      // We use the first WLS iteration results and correct them based on the ground truth position

      // and using a clock error computed from high elevation satellites. The first iteration is

      // used before satellite with high residuals being removed.

      if (isFirstWLS && truthLocationForCorrectedResidualComputationEcef != null) {

        // Snapshot the information needed before high residual satellites are removed

        System.arraycopy(

            ResidualCorrectionCalculator.calculateCorrectedResiduals(

                satPosPseudorangeResidualAndWeight,

                positionVelocitySolutionECEF.clone(),

                truthLocationForCorrectedResidualComputationEcef),

            0 /*source starting pos*/,

            pseudorangeResidualMeters,

            0 /*destination starting pos*/,

            GpsNavigationMessageStore.MAX_NUMBER_OF_SATELLITES /*length of elements*/);

        isFirstWLS = false;

      }

      repeatLeastSquare = false;

      int satsWithResidualBelowThreshold =

          satPosPseudorangeResidualAndWeight.pseudorangeResidualsMeters.length;

      // remove satellites that have residuals above RESIDUAL_TO_REPEAT_LEAST_SQUARE_METERS as they

      // worsen the position solution accuracy. If any satellite is removed, repeat the least square

      repeatLeastSquare =

          removeHighResidualSats(

              mutableSmoothedSatellitesToReceiverMeasurements,

              repeatLeastSquare,

              satPosPseudorangeResidualAndWeight,

              satsWithResidualBelowThreshold);


    } while (repeatLeastSquare);

    calculateGeoidMeters = false;


    // The computed ECEF position will be used next to compute the user velocity.

    // we calculate and fill in the user velocity solutions based on following equation:

    // Weight Matrix * GeometryMatrix * User Velocity Vector

    // = Weight Matrix * deltaPseudoRangeRateWeightedMps

    // Reference: Pratap Misra and Per Enge

    // "Global Positioning System: Signals, Measurements, and Performance" Page 218.


    // Get the number of satellite used in Geometry Matrix

    numberOfUsefulSatellites = geometryMatrix.getRowDimension();


    RealMatrix rangeRateMps = new Array2DRowRealMatrix(numberOfUsefulSatellites, 1);

    RealMatrix deltaPseudoRangeRateMps =

        new Array2DRowRealMatrix(numberOfUsefulSatellites, 1);

    RealMatrix pseudorangeRateWeight

        = new Array2DRowRealMatrix(numberOfUsefulSatellites, numberOfUsefulSatellites);


    // Correct the receiver time of week with the estimated receiver clock bias

    receiverGPSTowAtReceptionSeconds =

        receiverGPSTowAtReceptionSeconds - positionVelocitySolutionECEF[3] / SPEED_OF_LIGHT_MPS;


    int measurementCount = 0;


    // Calculate range rates

    for (int i = 0; i < GpsNavigationMessageStore.MAX_NUMBER_OF_SATELLITES; i++) {

      if (mutableSmoothedSatellitesToReceiverMeasurements.get(i) != null) {

        GpsEphemerisProto ephemeridesProto = getEphemerisForSatellite(navMessageProto, i + 1);


        double pseudorangeMeasurementMeters =

            mutableSmoothedSatellitesToReceiverMeasurements.get(i).pseudorangeMeters;

        GpsTimeOfWeekAndWeekNumber correctedTowAndWeek =

            calculateCorrectedTransmitTowAndWeek(ephemeridesProto, receiverGPSTowAtReceptionSeconds,

                receiverGPSWeek, pseudorangeMeasurementMeters);


        // Calculate satellite velocity

        PositionAndVelocity satPosECEFMetersVelocityMPS = SatellitePositionCalculator

            .calculateSatellitePositionAndVelocityFromEphemeris(

                ephemeridesProto,

                correctedTowAndWeek.gpsTimeOfWeekSeconds,

                correctedTowAndWeek.weekNumber,

                positionVelocitySolutionECEF[0],

                positionVelocitySolutionECEF[1],

                positionVelocitySolutionECEF[2]);


        // Calculate satellite clock error rate

        double satelliteClockErrorRateMps = SatelliteClockCorrectionCalculator.

            calculateSatClockCorrErrorRate(

                ephemeridesProto,

                correctedTowAndWeek.gpsTimeOfWeekSeconds,

                correctedTowAndWeek.weekNumber);


        // Fill in range rates. range rate = satellite velocity (dot product) line-of-sight vector

        rangeRateMps.setEntry(measurementCount, 0,  -1 * (

            satPosECEFMetersVelocityMPS.velocityXMetersPerSec

                * geometryMatrix.getEntry(measurementCount, 0)

                + satPosECEFMetersVelocityMPS.velocityYMetersPerSec

                * geometryMatrix.getEntry(measurementCount, 1)

                + satPosECEFMetersVelocityMPS.velocityZMetersPerSec

                * geometryMatrix.getEntry(measurementCount, 2)));


        deltaPseudoRangeRateMps.setEntry(measurementCount, 0,

            mutableSmoothedSatellitesToReceiverMeasurements.get(i).pseudorangeRateMps

                - rangeRateMps.getEntry(measurementCount, 0) + satelliteClockErrorRateMps

                - positionVelocitySolutionECEF[7]);


        // Calculate the velocity weight matrix by using 1 / square(Pseudorangerate Uncertainty)

        // along the diagonal

        pseudorangeRateWeight.setEntry(measurementCount, measurementCount,

            1 / (mutableSmoothedSatellitesToReceiverMeasurements

                .get(i).pseudorangeRateUncertaintyMps

                * mutableSmoothedSatellitesToReceiverMeasurements

                .get(i).pseudorangeRateUncertaintyMps));

        measurementCount++;

      }

    }


    RealMatrix weightedGeoMatrix = pseudorangeRateWeight.multiply(geometryMatrix);

    RealMatrix deltaPseudoRangeRateWeightedMps =

        pseudorangeRateWeight.multiply(deltaPseudoRangeRateMps);

    QRDecomposition qrdWeightedGeoMatrix = new QRDecomposition(weightedGeoMatrix);

    RealMatrix velocityMps

        = qrdWeightedGeoMatrix.getSolver().solve(deltaPseudoRangeRateWeightedMps);

    positionVelocitySolutionECEF[4] = velocityMps.getEntry(0, 0);

    positionVelocitySolutionECEF[5] = velocityMps.getEntry(1, 0);

    positionVelocitySolutionECEF[6] = velocityMps.getEntry(2, 0);

    positionVelocitySolutionECEF[7] = velocityMps.getEntry(3, 0);


    RealMatrix pseudorangeWeight

        = new LUDecomposition(

            new Array2DRowRealMatrix(satPosPseudorangeResidualAndWeight.covarianceMatrixMetersSquare

            )

    ).getSolver().getInverse();


    // Calculate and store the uncertainties of position and velocity in local ENU system in meters

    // and meters per second.

    double[] pvUncertainty =

        calculatePositionVelocityUncertaintyEnu(pseudorangeRateWeight, pseudorangeWeight,

            positionVelocitySolutionECEF);

    System.arraycopy(pvUncertainty,

        0 /*source starting pos*/,

        positionVelocityUncertaintyEnu,

        0 /*destination starting pos*/,

        6 /*length of elements*/);

  }


  /**

   * Calculates the position uncertainty in meters and the velocity uncertainty

   * in meters per second solution in local ENU system.

   *

   * <p> Reference: Global Positioning System: Signals, Measurements, and Performance

   * by Pratap Misra, Per Enge, Page 206 - 209.

   *

   * @param velocityWeightMatrix the velocity weight matrix

   * @param positionWeightMatrix the position weight matrix

   * @param positionVelocitySolution the position and velocity solution in ECEF

   * @return an array containing the position and velocity uncertainties in ENU coordinate system.

   *         [0-2] Enu uncertainty of position solution in meters.

   *         [3-5] Enu uncertainty of velocity solution in meters per second.

   */

  public double[] calculatePositionVelocityUncertaintyEnu(

      RealMatrix velocityWeightMatrix, RealMatrix positionWeightMatrix,

      double[] positionVelocitySolution){


    if (geometryMatrix == null){

      return null;

    }


    RealMatrix velocityH = calculateHMatrix(velocityWeightMatrix, geometryMatrix);

    RealMatrix positionH = calculateHMatrix(positionWeightMatrix, geometryMatrix);


    // Calculate the rotation Matrix to convert to local ENU system.

    RealMatrix rotationMatrix = new Array2DRowRealMatrix(4, 4);

    GeodeticLlaValues llaValues = Ecef2LlaConverter.convertECEFToLLACloseForm

        (positionVelocitySolution[0], positionVelocitySolution[1], positionVelocitySolution[2]);

    rotationMatrix.setSubMatrix(

        Ecef2EnuConverter.getRotationMatrix(llaValues.longitudeRadians,

            llaValues.latitudeRadians).getData(), 0, 0);

    rotationMatrix.setEntry(3, 3, 1);


    // Convert to local ENU by pre-multiply rotation matrix and multiply rotation matrix transposed

    velocityH = rotationMatrix.multiply(velocityH).multiply(rotationMatrix.transpose());

    positionH = rotationMatrix.multiply(positionH).multiply(rotationMatrix.transpose());


    // Return the square root of diagonal entries

    return new double[] {

        Math.sqrt(positionH.getEntry(0, 0)), Math.sqrt(positionH.getEntry(1, 1)),

        Math.sqrt(positionH.getEntry(2, 2)), Math.sqrt(velocityH.getEntry(0, 0)),

        Math.sqrt(velocityH.getEntry(1, 1)), Math.sqrt(velocityH.getEntry(2, 2))};

  }


  /**

   * Calculates the measurement connection matrix H as a function of weightMatrix and

   * geometryMatrix.

   *

   * <p> H = (geometryMatrixTransposed * Weight * geometryMatrix) ^ -1

   *

   * <p> Reference: Global Positioning System: Signals, Measurements, and Performance, P207

   * @param weightMatrix Weights for computing H Matrix

   * @return H Matrix

   */

  private RealMatrix calculateHMatrix

      (RealMatrix weightMatrix, RealMatrix geometryMatrix){


    RealMatrix tempH = geometryMatrix.transpose().multiply(weightMatrix).multiply(geometryMatrix);

    return new LUDecomposition(tempH).getSolver().getInverse();

  }


  /**

   * Applies weighted least square iterations and corrects to the position solution until correction

   * is below threshold. An exception is thrown if the maximum number of iterations:

   * {@value #MAXIMUM_NUMBER_OF_LEAST_SQUARE_ITERATIONS} is reached without convergence.

   */

  private SatellitesPositionPseudorangesResidualAndCovarianceMatrix applyWeightedLeastSquare(

      GpsNavMessageProto navMessageProto,

      List<GpsMeasurementWithRangeAndUncertainty> usefulSatellitesToReceiverMeasurements,

      double receiverGPSTowAtReceptionSeconds,

      int receiverGPSWeek,

      int dayOfYear1To366,

      double[] positionSolutionECEF,

      double[] deltaPositionMeters,

      boolean doAtmosphericCorrections,

      SatellitesPositionPseudorangesResidualAndCovarianceMatrix satPosPseudorangeResidualAndWeight,

      RealMatrix weightMatrixMetersMinus2)

      throws Exception {

    RealMatrix weightedGeometryMatrix;

    int numberOfIterations = 0;


    while ((Math.abs(deltaPositionMeters[0]) + Math.abs(deltaPositionMeters[1])

        + Math.abs(deltaPositionMeters[2])) >= LEAST_SQUARE_TOLERANCE_METERS) {

      // Apply ionospheric and tropospheric corrections only if the applied correction to

      // position is below a specific threshold

      if ((Math.abs(deltaPositionMeters[0]) + Math.abs(deltaPositionMeters[1])

          + Math.abs(deltaPositionMeters[2])) < ATMPOSPHERIC_CORRECTIONS_THRESHOLD_METERS) {

        doAtmosphericCorrections = true;

      }

      // Calculate satellites' positions, measurement residual per visible satellite and

      // weight matrix for the iterative least square

      satPosPseudorangeResidualAndWeight =

          calculateSatPosAndPseudorangeResidual(

              navMessageProto,

              usefulSatellitesToReceiverMeasurements,

              receiverGPSTowAtReceptionSeconds,

              receiverGPSWeek,

              dayOfYear1To366,

              positionSolutionECEF,

              doAtmosphericCorrections);


      // Calculate the geometry matrix according to "Global Positioning System: Theory and

      // Applications", Parkinson and Spilker page 413

      geometryMatrix = new Array2DRowRealMatrix(calculateGeometryMatrix(

          satPosPseudorangeResidualAndWeight.satellitesPositionsMeters, positionSolutionECEF));

      // Apply weighted least square only if the covariance matrix is

      // not singular (has a non-zero determinant), otherwise apply ordinary least square.

      // The reason is to ignore reported signal to noise ratios by the receiver that can

      // lead to such singularities

      if (weightMatrixMetersMinus2 == null) {

        weightedGeometryMatrix = geometryMatrix;

      } else {

        RealMatrix hMatrix =

            calculateHMatrix(weightMatrixMetersMinus2, geometryMatrix);

        weightedGeometryMatrix = hMatrix.multiply(geometryMatrix.transpose())

            .multiply(weightMatrixMetersMinus2);

      }


      // Equation 9 page 413 from "Global Positioning System: Theory and Applicaitons",

      // Parkinson and Spilker

      deltaPositionMeters =

          GpsMathOperations.matrixByColVectMultiplication(

              weightedGeometryMatrix.getData(),

              satPosPseudorangeResidualAndWeight.pseudorangeResidualsMeters);


      // Apply corrections to the position estimate

      positionSolutionECEF[0] += deltaPositionMeters[0];

      positionSolutionECEF[1] += deltaPositionMeters[1];

      positionSolutionECEF[2] += deltaPositionMeters[2];

      positionSolutionECEF[3] += deltaPositionMeters[3];

      numberOfIterations++;

      Preconditions.checkArgument(numberOfIterations <= MAXIMUM_NUMBER_OF_LEAST_SQUARE_ITERATIONS,

          "Maximum number of least square iterations reached without convergance...");

    }

    return satPosPseudorangeResidualAndWeight;

  }


  /**

   * Removes satellites that have residuals above {@value #RESIDUAL_TO_REPEAT_LEAST_SQUARE_METERS}

   * from the {@code usefulSatellitesToReceiverMeasurements} list. Returns true if any satellite is

   * removed.

   */

  private boolean removeHighResidualSats(

      List<GpsMeasurementWithRangeAndUncertainty> usefulSatellitesToReceiverMeasurements,

      boolean repeatLeastSquare,

      SatellitesPositionPseudorangesResidualAndCovarianceMatrix satPosPseudorangeResidualAndWeight,

      int satsWithResidualBelowThreshold) {


    for (int i = 0; i < satPosPseudorangeResidualAndWeight.pseudorangeResidualsMeters.length; i++) {

      if (satsWithResidualBelowThreshold > MINIMUM_NUMER_OF_SATELLITES) {

        if (Math.abs(satPosPseudorangeResidualAndWeight.pseudorangeResidualsMeters[i]) 

            > RESIDUAL_TO_REPEAT_LEAST_SQUARE_METERS) {

          int prn = satPosPseudorangeResidualAndWeight.satellitePRNs[i];

          usefulSatellitesToReceiverMeasurements.set(prn - 1, null);

          satsWithResidualBelowThreshold--;

          repeatLeastSquare = true;

        }

      }

    }

    return repeatLeastSquare;

  }


  /**

   * Calculates position of all visible satellites and pseudorange measurement residual

   * (difference of measured to predicted pseudoranges) needed for the least square computation. The

   * result is stored in an instance of {@link

   * SatellitesPositionPseudorangesResidualAndCovarianceMatrix}

   *

   * @param navMeassageProto parameters of the navigation message

   * @param usefulSatellitesToReceiverMeasurements Map of useful satellite PRN to {@link

   *     GpsMeasurementWithRangeAndUncertainty} containing receiver measurements for computing the

   *     position solution

   * @param receiverGPSTowAtReceptionSeconds Receiver estimate of GPS time of week (seconds)

   * @param receiverGpsWeek Receiver estimate of GPS week (0-1024+)

   * @param dayOfYear1To366 The day of the year between 1 and 366

   * @param userPositionECEFMeters receiver ECEF position in meters

   * @param doAtmosphericCorrections boolean indicating if atmospheric range corrections should be

   *     applied

   * @return SatellitesPositionPseudorangesResidualAndCovarianceMatrix Object containing satellite

   *     prns, satellite positions in ECEF, pseudorange residuals and covariance matrix.

   */

  public SatellitesPositionPseudorangesResidualAndCovarianceMatrix

      calculateSatPosAndPseudorangeResidual(

          GpsNavMessageProto navMeassageProto,

          List<GpsMeasurementWithRangeAndUncertainty> usefulSatellitesToReceiverMeasurements,

          double receiverGPSTowAtReceptionSeconds,

          int receiverGpsWeek,

          int dayOfYear1To366,

          double[] userPositionECEFMeters,

          boolean doAtmosphericCorrections)

          throws Exception {

    int numberOfUsefulSatellites =

        getNumberOfUsefulSatellites(usefulSatellitesToReceiverMeasurements);

    // deltaPseudorange is the pseudorange measurement residual

    double[] deltaPseudorangesMeters = new double[numberOfUsefulSatellites];

    double[][] satellitesPositionsECEFMeters = new double[numberOfUsefulSatellites][3];


    // satellite PRNs

    int[] satellitePRNs = new int[numberOfUsefulSatellites];


    // Ionospheric model parameters

    double[] alpha =

            {navMeassageProto.iono.alpha[0], navMeassageProto.iono.alpha[1],

                    navMeassageProto.iono.alpha[2], navMeassageProto.iono.alpha[3]};

    double[] beta = {navMeassageProto.iono.beta[0], navMeassageProto.iono.beta[1],

            navMeassageProto.iono.beta[2], navMeassageProto.iono.beta[3]};

    // Weight matrix for the weighted least square

    RealMatrix covarianceMatrixMetersSquare =

        new Array2DRowRealMatrix(numberOfUsefulSatellites, numberOfUsefulSatellites);

    calculateSatPosAndResiduals(

        navMeassageProto,

        usefulSatellitesToReceiverMeasurements,

        receiverGPSTowAtReceptionSeconds,

        receiverGpsWeek,

        dayOfYear1To366,

        userPositionECEFMeters,

        doAtmosphericCorrections,

        deltaPseudorangesMeters,

        satellitesPositionsECEFMeters,

        satellitePRNs,

        alpha,

        beta,

        covarianceMatrixMetersSquare);


    return new SatellitesPositionPseudorangesResidualAndCovarianceMatrix(satellitePRNs,

        satellitesPositionsECEFMeters, deltaPseudorangesMeters,

        covarianceMatrixMetersSquare.getData());

  }


  /**

   * Calculates and fill the position of all visible satellites:

   * {@code satellitesPositionsECEFMeters}, pseudorange measurement residual (difference of

   * measured to predicted pseudoranges): {@code deltaPseudorangesMeters} and covariance matrix from

   * the weighted least square: {@code covarianceMatrixMetersSquare}. An array of the satellite PRNs

   * {@code satellitePRNs} is as well filled.

   */

  private void 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)

      throws Exception {

    // user position without the clock estimate

    double[] userPositionTempECEFMeters =

        {userPositionECEFMeters[0], userPositionECEFMeters[1], userPositionECEFMeters[2]};

    int satsCounter = 0;

    for (int i = 0; i < GpsNavigationMessageStore.MAX_NUMBER_OF_SATELLITES; i++) {

      if (usefulSatellitesToReceiverMeasurements.get(i) != null) {

        GpsEphemerisProto ephemeridesProto = getEphemerisForSatellite(navMeassageProto, i + 1);

        // Correct the receiver time of week with the estimated receiver clock bias

        receiverGPSTowAtReceptionSeconds =

            receiverGPSTowAtReceptionSeconds - userPositionECEFMeters[3] / SPEED_OF_LIGHT_MPS;


        double pseudorangeMeasurementMeters =

            usefulSatellitesToReceiverMeasurements.get(i).pseudorangeMeters;

        double pseudorangeUncertaintyMeters =

            usefulSatellitesToReceiverMeasurements.get(i).pseudorangeUncertaintyMeters;


        // Assuming uncorrelated pseudorange measurements, the covariance matrix will be diagonal as

        // follows

        covarianceMatrixMetersSquare.setEntry(satsCounter, satsCounter,

            pseudorangeUncertaintyMeters * pseudorangeUncertaintyMeters);


        // Calculate time of week at transmission time corrected with the satellite clock drift

        GpsTimeOfWeekAndWeekNumber correctedTowAndWeek =

            calculateCorrectedTransmitTowAndWeek(ephemeridesProto, receiverGPSTowAtReceptionSeconds,

                receiverGpsWeek, pseudorangeMeasurementMeters);


        // calculate satellite position and velocity

        PositionAndVelocity satPosECEFMetersVelocityMPS = SatellitePositionCalculator

            .calculateSatellitePositionAndVelocityFromEphemeris(ephemeridesProto,

                correctedTowAndWeek.gpsTimeOfWeekSeconds, correctedTowAndWeek.weekNumber,

                userPositionECEFMeters[0], userPositionECEFMeters[1], userPositionECEFMeters[2]);


        satellitesPositionsECEFMeters[satsCounter][0] = satPosECEFMetersVelocityMPS.positionXMeters;

        satellitesPositionsECEFMeters[satsCounter][1] = satPosECEFMetersVelocityMPS.positionYMeters;

        satellitesPositionsECEFMeters[satsCounter][2] = satPosECEFMetersVelocityMPS.positionZMeters;


        // Calculate ionospheric and tropospheric corrections

        double ionosphericCorrectionMeters;

        double troposphericCorrectionMeters;

        if (doAtmosphericCorrections) {

          ionosphericCorrectionMeters =

              IonosphericModel.ionoKloboucharCorrectionSeconds(

                      userPositionTempECEFMeters,

                      satellitesPositionsECEFMeters[satsCounter],

                      correctedTowAndWeek.gpsTimeOfWeekSeconds,

                      alpha,

                      beta,

                      IonosphericModel.L1_FREQ_HZ)

                  * SPEED_OF_LIGHT_MPS;


          troposphericCorrectionMeters =

              calculateTroposphericCorrectionMeters(

                  dayOfYear1To366,

                  satellitesPositionsECEFMeters,

                  userPositionTempECEFMeters,

                  satsCounter);

        } else {

          troposphericCorrectionMeters = 0.0;

          ionosphericCorrectionMeters = 0.0;

        }

        double predictedPseudorangeMeters =

            calculatePredictedPseudorange(userPositionECEFMeters, satellitesPositionsECEFMeters,

                userPositionTempECEFMeters, satsCounter, ephemeridesProto, correctedTowAndWeek,

                ionosphericCorrectionMeters, troposphericCorrectionMeters);


        // Pseudorange residual (difference of measured to predicted pseudoranges)

        deltaPseudorangesMeters[satsCounter] =

            pseudorangeMeasurementMeters - predictedPseudorangeMeters;


        // Satellite PRNs

        satellitePRNs[satsCounter] = i + 1;

        satsCounter++;

      }

    }

  }


  /** Searches ephemerides list for the ephemeris associated with current satellite in process */

  private GpsEphemerisProto getEphemerisForSatellite(GpsNavMessageProto navMeassageProto,

                                                     int satPrn) {

    List<GpsEphemerisProto> ephemeridesList

            = new ArrayList<GpsEphemerisProto>(Arrays.asList(navMeassageProto.ephemerids));

    GpsEphemerisProto ephemeridesProto = null;

    int ephemerisPrn = 0;

    for (GpsEphemerisProto ephProtoFromList : ephemeridesList) {

      ephemerisPrn = ephProtoFromList.prn;

      if (ephemerisPrn == satPrn) {

        ephemeridesProto = ephProtoFromList;

        break;

      }

    }

    return ephemeridesProto;

  }


  /** Calculates predicted pseudorange in meters */

  private double calculatePredictedPseudorange(

      double[] userPositionECEFMeters,

      double[][] satellitesPositionsECEFMeters,

      double[] userPositionNoClockECEFMeters,

      int satsCounter,

      GpsEphemerisProto ephemeridesProto,

      GpsTimeOfWeekAndWeekNumber correctedTowAndWeek,

      double ionosphericCorrectionMeters,

      double troposphericCorrectionMeters)

      throws Exception {

    // Calcualte the satellite clock drift

    double satelliteClockCorrectionMeters =

        SatelliteClockCorrectionCalculator.calculateSatClockCorrAndEccAnomAndTkIteratively(

                ephemeridesProto,

                correctedTowAndWeek.gpsTimeOfWeekSeconds,

                correctedTowAndWeek.weekNumber)

            .satelliteClockCorrectionMeters;


    double satelliteToUserDistanceMeters =

        GpsMathOperations.vectorNorm(GpsMathOperations.subtractTwoVectors(

            satellitesPositionsECEFMeters[satsCounter], userPositionNoClockECEFMeters));

    // Predicted pseudorange

    double predictedPseudorangeMeters =

        satelliteToUserDistanceMeters - satelliteClockCorrectionMeters + ionosphericCorrectionMeters

            + troposphericCorrectionMeters + userPositionECEFMeters[3];

    return predictedPseudorangeMeters;

  }


  /** Calculates the Gps tropospheric correction in meters */

  private double calculateTroposphericCorrectionMeters(int dayOfYear1To366,

      double[][] satellitesPositionsECEFMeters, double[] userPositionTempECEFMeters,

      int satsCounter) {

    double troposphericCorrectionMeters;

    TopocentricAEDValues elevationAzimuthDist =

        EcefToTopocentricConverter.convertCartesianToTopocentericRadMeters(

            userPositionTempECEFMeters, GpsMathOperations.subtractTwoVectors(

                satellitesPositionsECEFMeters[satsCounter], userPositionTempECEFMeters));


    GeodeticLlaValues lla =

        Ecef2LlaConverter.convertECEFToLLACloseForm(userPositionTempECEFMeters[0],

            userPositionTempECEFMeters[1], userPositionTempECEFMeters[2]);


    // Geoid of the area where the receiver is located is calculated once and used for the

    // rest of the dataset as it change very slowly over wide area. This to save the delay

    // associated with accessing Google Elevation API. We assume this very first iteration of WLS

    // will compute the correct altitude above the ellipsoid of the ground at the latitude and

    // longitude

    if (calculateGeoidMeters) {

      double elevationAboveSeaLevelMeters = 0;

      if (elevationApiHelper == null){

        System.out.println("No Google API key is set. Elevation above sea level is set to "

            + "default 0 meters. This may cause inaccuracy in tropospheric correction.");

      } else {

        try {

          elevationAboveSeaLevelMeters = elevationApiHelper

              .getElevationAboveSeaLevelMeters(

                  Math.toDegrees(lla.latitudeRadians), Math.toDegrees(lla.longitudeRadians)

              );

        } catch (Exception e){

          e.printStackTrace();

          System.out.println("Error when getting elevation from Google Server. "

              + "Could be wrong Api key or network error. Elevation above sea level is set to "

              + "default 0 meters. This may cause inaccuracy in tropospheric correction.");

        }

      }


      geoidHeightMeters = ElevationApiHelper.calculateGeoidHeightMeters(

              lla.altitudeMeters,

              elevationAboveSeaLevelMeters

      );

      troposphericCorrectionMeters = TroposphericModelEgnos.calculateTropoCorrectionMeters(

          elevationAzimuthDist.elevationRadians, lla.latitudeRadians, elevationAboveSeaLevelMeters,

          dayOfYear1To366);

    } else {

      troposphericCorrectionMeters = TroposphericModelEgnos.calculateTropoCorrectionMeters(

          elevationAzimuthDist.elevationRadians, lla.latitudeRadians,

          lla.altitudeMeters - geoidHeightMeters, dayOfYear1To366);

    }

    return troposphericCorrectionMeters;

  }


  /**

   * Gets the number of useful satellites from a list of

   * {@link GpsMeasurementWithRangeAndUncertainty}.

   */

  private int getNumberOfUsefulSatellites(

      List<GpsMeasurementWithRangeAndUncertainty> usefulSatellitesToReceiverMeasurements) {

    // calculate the number of useful satellites

    int numberOfUsefulSatellites = 0;

    for (int i = 0; i < usefulSatellitesToReceiverMeasurements.size(); i++) {

      if (usefulSatellitesToReceiverMeasurements.get(i) != null) {

        numberOfUsefulSatellites++;

      }

    }

    return numberOfUsefulSatellites;

  }


  /**

   * Computes the GPS time of week at the time of transmission and as well the corrected GPS week

   * taking into consideration week rollover. The returned GPS time of week is corrected by the

   * computed satellite clock drift. The result is stored in an instance of

   * {@link GpsTimeOfWeekAndWeekNumber}

   *

   * @param ephemerisProto parameters of the navigation message

   * @param receiverGpsTowAtReceptionSeconds Receiver estimate of GPS time of week when signal was

   *        received (seconds)

   * @param receiverGpsWeek Receiver estimate of GPS week (0-1024+)

   * @param pseudorangeMeters Measured pseudorange in meters

   * @return GpsTimeOfWeekAndWeekNumber Object containing Gps time of week and week number.

   */

  private static GpsTimeOfWeekAndWeekNumber calculateCorrectedTransmitTowAndWeek(

      GpsEphemerisProto ephemerisProto, double receiverGpsTowAtReceptionSeconds,

      int receiverGpsWeek, double pseudorangeMeters) throws Exception {

    // GPS time of week at time of transmission: Gps time corrected for transit time (page 98 ICD

    // GPS 200)

    double receiverGpsTowAtTimeOfTransmission =

        receiverGpsTowAtReceptionSeconds - pseudorangeMeters / SPEED_OF_LIGHT_MPS;


    // Adjust for week rollover

    if (receiverGpsTowAtTimeOfTransmission < 0) {

      receiverGpsTowAtTimeOfTransmission += SECONDS_IN_WEEK;

      receiverGpsWeek -= 1;

    } else if (receiverGpsTowAtTimeOfTransmission > SECONDS_IN_WEEK) {

      receiverGpsTowAtTimeOfTransmission -= SECONDS_IN_WEEK;

      receiverGpsWeek += 1;

    }


    // Compute the satellite clock correction term (Seconds)

    double clockCorrectionSeconds =

        SatelliteClockCorrectionCalculator.calculateSatClockCorrAndEccAnomAndTkIteratively(

            ephemerisProto, receiverGpsTowAtTimeOfTransmission,

            receiverGpsWeek).satelliteClockCorrectionMeters / SPEED_OF_LIGHT_MPS;


    // Correct with the satellite clock correction term

    double receiverGpsTowAtTimeOfTransmissionCorrectedSec =

        receiverGpsTowAtTimeOfTransmission + clockCorrectionSeconds;


    // Adjust for week rollover due to satellite clock correction

    if (receiverGpsTowAtTimeOfTransmissionCorrectedSec < 0.0) {

      receiverGpsTowAtTimeOfTransmissionCorrectedSec += SECONDS_IN_WEEK;

      receiverGpsWeek -= 1;

    }

    if (receiverGpsTowAtTimeOfTransmissionCorrectedSec > SECONDS_IN_WEEK) {

      receiverGpsTowAtTimeOfTransmissionCorrectedSec -= SECONDS_IN_WEEK;

      receiverGpsWeek += 1;

    }

    return new GpsTimeOfWeekAndWeekNumber(receiverGpsTowAtTimeOfTransmissionCorrectedSec,

        receiverGpsWeek);

  }


  /**

   * Calculates the Geometry matrix (describing user to satellite geometry) given a list of

   * satellite positions in ECEF coordinates in meters and the user position in ECEF in meters.

   *

   * <p>The geometry matrix has four columns, and rows equal to the number of satellites. For each

   * of the rows (i.e. for each of the satellites used), the columns are filled with the normalized

   * line–of-sight vectors and 1 s for the fourth column.

   *

   * <p>Source: Parkinson, B.W., Spilker Jr., J.J.: ‘Global positioning system: theory and

   * applications’ page 413

   */

  private static double[][] calculateGeometryMatrix(double[][] satellitePositionsECEFMeters,

      double[] userPositionECEFMeters) {


    double[][] geometeryMatrix = new double[satellitePositionsECEFMeters.length][4];

    for (int i = 0; i < satellitePositionsECEFMeters.length; i++) {

      geometeryMatrix[i][3] = 1;

    }

    // iterate over all satellites

    for (int i = 0; i < satellitePositionsECEFMeters.length; i++) {

      double[] r = {satellitePositionsECEFMeters[i][0] - userPositionECEFMeters[0],

          satellitePositionsECEFMeters[i][1] - userPositionECEFMeters[1],

          satellitePositionsECEFMeters[i][2] - userPositionECEFMeters[2]};

      double norm = Math.sqrt(Math.pow(r[0], 2) + Math.pow(r[1], 2) + Math.pow(r[2], 2));

      for (int j = 0; j < 3; j++) {

        geometeryMatrix[i][j] =

            (userPositionECEFMeters[j] - satellitePositionsECEFMeters[i][j]) / norm;

      }

    }

    return geometeryMatrix;

  }


  /**

   * Class containing satellites' PRNs, satellites' positions in ECEF meters, the pseudorange

   * residual per visible satellite in meters and the covariance matrix of the

   * pseudoranges in meters square

   */

  protected static class SatellitesPositionPseudorangesResidualAndCovarianceMatrix {


    /** Satellites' PRNs */

    protected final int[] satellitePRNs;


    /** ECEF positions (meters) of useful satellites */

    protected final double[][] satellitesPositionsMeters;


    /** Pseudorange measurement residuals (difference of measured to predicted pseudoranges) */

    protected final double[] pseudorangeResidualsMeters;


    /** Pseudorange covariance Matrix for the weighted least squares (meters square) */

    protected final double[][] covarianceMatrixMetersSquare;


    /** Constructor */

    private SatellitesPositionPseudorangesResidualAndCovarianceMatrix(int[] satellitePRNs,

        double[][] satellitesPositionsMeters, double[] pseudorangeResidualsMeters,

        double[][] covarianceMatrixMetersSquare) {

      this.satellitePRNs = satellitePRNs;

      this.satellitesPositionsMeters = satellitesPositionsMeters;

      this.pseudorangeResidualsMeters = pseudorangeResidualsMeters;

      this.covarianceMatrixMetersSquare = covarianceMatrixMetersSquare;

    }


  }


  /**

   * Class containing GPS time of week in seconds and GPS week number

   */

  private static class GpsTimeOfWeekAndWeekNumber {

    /** GPS time of week in seconds */

    private final double gpsTimeOfWeekSeconds;


    /** GPS week number */

    private final int weekNumber;


    /** Constructor */

    private GpsTimeOfWeekAndWeekNumber(double gpsTimeOfWeekSeconds, int weekNumber) {

      this.gpsTimeOfWeekSeconds = gpsTimeOfWeekSeconds;

      this.weekNumber = weekNumber;

    }

  }

  

  /**

   * Uses the common reception time approach to calculate pseudoranges from the time of week

   * measurements reported by the receiver according to http://cdn.intechopen.com/pdfs-wm/27712.pdf.

   * As well computes the pseudoranges uncertainties for each input satellite

   */

  @VisibleForTesting

  static List<GpsMeasurementWithRangeAndUncertainty> computePseudorangeAndUncertainties(

      List<GpsMeasurement> usefulSatellitesToReceiverMeasurements,

      Long[] usefulSatellitesToTOWNs,

      long largestTowNs) {


    List<GpsMeasurementWithRangeAndUncertainty> usefulSatellitesToPseudorangeMeasurements =

        Arrays.asList(

            new GpsMeasurementWithRangeAndUncertainty

                [GpsNavigationMessageStore.MAX_NUMBER_OF_SATELLITES]);

    for (int i = 0; i < GpsNavigationMessageStore.MAX_NUMBER_OF_SATELLITES; i++) {

      if (usefulSatellitesToTOWNs[i] != null) {

        double deltai = largestTowNs - usefulSatellitesToTOWNs[i];

        double pseudorangeMeters =

            (AVERAGE_TRAVEL_TIME_SECONDS + deltai * SECONDS_PER_NANO) * SPEED_OF_LIGHT_MPS;


        double signalToNoiseRatioLinear =

            Math.pow(10, usefulSatellitesToReceiverMeasurements.get(i).signalToNoiseRatioDb / 10.0);

        // From Global Positoning System book, Misra and Enge, page 416, the uncertainty of the

        // pseudorange measurement is calculated next.

        // For GPS C/A code chip width Tc = 1 microseconds. Narrow correlator with spacing d = 0.1

        // chip and an average time of DLL correlator T of 20 milliseconds are used.

        double sigmaMeters =

            SPEED_OF_LIGHT_MPS

                * GPS_CHIP_WIDTH_T_C_SEC

                * Math.sqrt(

                    GPS_CORRELATOR_SPACING_IN_CHIPS

                        / (4 * GPS_DLL_AVERAGING_TIME_SEC * signalToNoiseRatioLinear));

        usefulSatellitesToPseudorangeMeasurements.set(

            i,

            new GpsMeasurementWithRangeAndUncertainty(

                usefulSatellitesToReceiverMeasurements.get(i), pseudorangeMeters, sigmaMeters));

      }

    }

    return usefulSatellitesToPseudorangeMeasurements;

  }


}


阅读次数: 2467

下一篇: 群延迟
上一篇: PseudorangePositionVelocityFromRealTimeEvents

尚无评论!

返回上一页面