Example #1
0
        public override void calculateCorrection(Time currentTime, Coordinates <Matrix> approximatedPose, SatellitePosition satelliteCoordinates, NavigationProducer navigationProducer, Location initialLocation)
        {
            DateTime foo      = DateTime.Now;
            long     unixTime = ((DateTimeOffset)foo).ToUnixTimeSeconds();
            IonoGps  iono     = navigationProducer.getIono(unixTime, initialLocation);

            if (iono.getBeta(0) == 0)
            {
                correctionValue = 0.0;
            }
            else
            {
                // Compute the elevation and azimuth angles for each satellite
                TopocentricCoordinates <Matrix> topo = new TopocentricCoordinates <Matrix>();
                topo.computeTopocentric(approximatedPose, satelliteCoordinates);

                // Assign the elevation and azimuth information to new variables
                double elevation = topo.getElevation();
                double azimuth   = topo.getAzimuth();

                double ionoCorr = 0;

                if (iono == null)
                {
                    return;
                }
                //		    double a0 = navigation.getIono(currentTime.getMsec(),0);
                //		    double a1 = navigation.getIono(currentTime.getMsec(),1);
                //		    double a2 = navigation.getIono(currentTime.getMsec(),2);
                //		    double a3 = navigation.getIono(currentTime.getMsec(),3);
                //		    double b0 = navigation.getIono(currentTime.getMsec(),4);
                //		    double b1 = navigation.getIono(currentTime.getMsec(),5);
                //		    double b2 = navigation.getIono(currentTime.getMsec(),6);
                //		    double b3 = navigation.getIono(currentTime.getMsec(),7);

                elevation = Math.Abs(elevation);

                // Parameter conversion to semicircles
                double lon = approximatedPose.getGeodeticLongitude() / 180; // geod.get(0)
                double lat = approximatedPose.getGeodeticLatitude() / 180;  //geod.get(1)
                azimuth   = azimuth / 180;
                elevation = elevation / 180;

                // Klobuchar algorithm

                // Compute the slant factor
                double f = 1 + 16 * Math.Pow((0.53 - elevation), 3);

                // Compute the earth-centred angle
                double psi = 0.0137 / (elevation + 0.11) - 0.022;

                // Compute the latitude of the Ionospheric Pierce Point (IPP)
                double phi = lat + psi * Math.Cos(azimuth * Math.PI);

                if (phi > 0.416)
                {
                    phi = 0.416;
                }
                if (phi < -0.416)
                {
                    phi = -0.416;
                }

                // Compute the longitude of the IPP
                double lambda = lon + (psi * Math.Sin(azimuth * Math.PI))
                                / Math.Cos(phi * Math.PI);

                // Find the geomagnetic latitude of the IPP
                double ro = phi + 0.064 * Math.Cos((lambda - 1.617) * Math.PI);

                // Find the local time at the IPP
                double t = lambda * 43200 + unixTime;

                while (t >= 86400)
                {
                    t = t - 86400;
                }

                while (t < 0)
                {
                    t = t + 86400;
                }

                // Compute the period of ionospheric delay
                double p = iono.getBeta(0) + iono.getBeta(1) * ro + iono.getBeta(2) * Math.Pow(ro, 2) + iono.getBeta(3) * Math.Pow(ro, 3);

                if (p < 72000)
                {
                    p = 72000;
                }

                // Compute the amplitude of ionospheric delay
                double a = iono.getAlpha(0) + iono.getAlpha(1) * ro + iono.getAlpha(2) * Math.Pow(ro, 2) + iono.getAlpha(3) * Math.Pow(ro, 3);

                if (a < 0)
                {
                    a = 0;
                }

                // Compute the phase of ionospheric delay
                double x = (2 * Math.PI * (t - 50400)) / p;

                // Compute the ionospheric correction
                if (Math.Abs(x) < 1.57)
                {
                    ionoCorr = Constants.SPEED_OF_LIGHT
                               * f
                               * (5e-9 + a
                                  * (1 - (Math.Pow(x, 2)) / 2 + (Math.Pow(x, 4)) / 24));
                }
                else
                {
                    ionoCorr = Constants.SPEED_OF_LIGHT * f * 5e-9;
                }

                correctionValue = ionoCorr;
            }
        }
Example #2
0
        public override Coordinates <Matrix> calculatePose(Constellation constellation)
        {
            int CONSTELLATION_SIZE = constellation.getUsedConstellationSize();

            // Initialize matrices for data storage

            SimpleMatrix <Matrix> rxPosSimpleVector = Constellation.getRxPosAsVector(constellation.getRxPos());
            SimpleMatrix <Matrix> satPosMat         = new SimpleMatrix <Matrix>(CONSTELLATION_SIZE, 3);
            SimpleMatrix <Matrix> tropoCorr         = new SimpleMatrix <Matrix>(CONSTELLATION_SIZE, 1);
            SimpleMatrix <Matrix> svClkBias         = new SimpleMatrix <Matrix>(CONSTELLATION_SIZE, 1);
            SimpleMatrix <Matrix> ionoCorr          = new SimpleMatrix <Matrix>(CONSTELLATION_SIZE, 1);
            SimpleMatrix <Matrix> shapiroCorr       = new SimpleMatrix <Matrix>(CONSTELLATION_SIZE, 1);
            SimpleMatrix <Matrix> sigma2            = new SimpleMatrix <Matrix>(CONSTELLATION_SIZE, 1);
            SimpleMatrix <Matrix> prVect            = new SimpleMatrix <Matrix>(CONSTELLATION_SIZE, 1);
            SimpleMatrix <Matrix> vcvMeasurement;
            SimpleMatrix <Matrix> PDOP = new SimpleMatrix <Matrix>(1, 1);

            double elevation, measVar, measVarC1;
            int    CN0;

            TopocentricCoordinates <Matrix> topo = new TopocentricCoordinates <Matrix>();
            Coordinates <Matrix>            origin; // = new Coordinates<Matrix>(){};
            Coordinates <Matrix>            target; // = new Coordinates<Matrix>(){};

            ///////////////////////////// SV Coordinates<Matrix>/velocities + PR corrections computation ////////////////////////////////////////////////////
            try
            {
                for (int ii = 0; ii < CONSTELLATION_SIZE; ii++)
                {
                    // Set the measurements into a vector
                    prVect.set(ii, constellation.getSatellite(ii).getPseudorange());

                    // Compute the satellite Coordinates<Matrix>
                    svClkBias.set(ii, constellation.getSatellite(ii).getClockBias());

                    ///////////////////////////// PR corrections computations ////////////////////////////////////////////////////


                    // Assign the computed SV Coordinates<Matrix> into a matrix
                    satPosMat.set(ii, 0, constellation.getSatellite(ii).getSatellitePosition().getX());
                    satPosMat.set(ii, 1, constellation.getSatellite(ii).getSatellitePosition().getY());
                    satPosMat.set(ii, 2, constellation.getSatellite(ii).getSatellitePosition().getZ());


                    // Compute the elevation and azimuth angles for each satellite
                    origin = Coordinates <Matrix> .globalXYZInstance(rxPosSimpleVector.get(0), rxPosSimpleVector.get(1), rxPosSimpleVector.get(2));

                    target = Coordinates <Matrix> .globalXYZInstance(satPosMat.get(ii, 0), satPosMat.get(ii, 1), satPosMat.get(ii, 2));

                    //                origin.setXYZ(rxPosSimpleVector.get(0), rxPosSimpleVector.get(1),rxPosSimpleVector.get(2));
                    //                target.setXYZ(satPosMat.get(ii, 0),satPosMat.get(ii, 1),satPosMat.get(ii, 2) );
                    topo.computeTopocentric(origin, target);
                    elevation = topo.getElevation() * (Math.PI / 180.0);

                    // Set the variance of the measurement for each satellite
                    measVar = sigma2Meas * Math.Pow(a + b * Math.Exp(-elevation / 10.0), 2);
                    sigma2.set(ii, measVar);

                    ///////////////////////////// Printing results  ////////////////////////////////////////////////////

                    // Print the computed satellite Coordinates<Matrix> \ velocities

                    /*
                     * System.out.println();
                     * System.out.println();
                     * System.out.println("The GPS ECEF Coordinates<Matrix> of " + "SV" + constellation.getSatellite(ii).getSatId() + " are:");
                     * System.out.printf("%.4f", satPosMat.get(ii, 0));
                     * System.out.println();
                     * System.out.printf("%.4f", satPosMat.get(ii, 1));
                     * System.out.println();
                     * System.out.printf("%.4f", satPosMat.get(ii, 2));
                     * System.out.println();
                     * System.out.printf("Tropo corr: %.4f", tropoCorr.get(ii));
                     * System.out.println();
                     * System.out.printf("Iono corr: %.4f", ionoCorr.get(ii));
                     * System.out.println();
                     */
                    //System.out.printf("Shapiro corr: %.4f", shapiroCorr.get(ii));
                }
            }
            catch (Exception e)
            {
                //e.printStackTrace();

                //if (e.getClass() == IndexOutOfBoundsException.class){
                //    Log.e(TAG, "calculatePose: Satellites cleared before calculating result!");
                //}

                constellation.setRxPos(ZERO_POSE); // Right at the edge of the plot
                rxPosSimpleVector = Constellation.getRxPosAsVector(constellation.getRxPos());
                return(Coordinates <Matrix> .globalXYZInstance(rxPosSimpleVector.get(0), rxPosSimpleVector.get(1), rxPosSimpleVector.get(2)));
            }


            /*
             * [WEIGHTED LEAST SQUARES] Determination of the position + clock bias
             */

            try
            {
                // VCV matrix of the pseudorange measurements
                vcvMeasurement = ((SimpleBase <SimpleMatrix <Matrix>, Matrix>)sigma2).diag();

                SimpleMatrix <Matrix> W = vcvMeasurement.invert();

                // Initialization of the required matrices and vectors
                SimpleMatrix <Matrix> xHat     = new SimpleMatrix <Matrix>(4, 1);                  // vector holding the WLS estimates
                SimpleMatrix <Matrix> z        = new SimpleMatrix <Matrix>(CONSTELLATION_SIZE, 1); // observation vector
                SimpleMatrix <Matrix> H        = new SimpleMatrix <Matrix>(CONSTELLATION_SIZE, 4); // observation matrix
                SimpleMatrix <Matrix> distPred = new SimpleMatrix <Matrix>(CONSTELLATION_SIZE, 1); // predicted distances
                SimpleMatrix <Matrix> measPred = new SimpleMatrix <Matrix>(CONSTELLATION_SIZE, 1); // predicted measurements


                // Start the estimation (10 loops)
                for (int iter = 0; iter < NUMBER_OF_ITERATIONS; iter++)
                {
                    // Calculation of the components within the observation matrix (H)
                    for (int k = 0; k < CONSTELLATION_SIZE; k++)
                    {
                        // Computation of the geometric distance
                        distPred.set(k, Math.Sqrt(
                                         Math.Pow(satPosMat.get(k, 0) - rxPosSimpleVector.get(0), 2)
                                         + Math.Pow(satPosMat.get(k, 1) - rxPosSimpleVector.get(1), 2)
                                         + Math.Pow(satPosMat.get(k, 2) - rxPosSimpleVector.get(2), 2)
                                         ));


                        // Measurement prediction
                        measPred.set(k, distPred.get(k)
                                     + constellation.getSatellite(k).getAccumulatedCorrection() - svClkBias.get(k));

                        // Compute the observation matrix (H)
                        H.set(k, 0, (constellation.getRxPos().getX() - satPosMat.get(k, 0)) / distPred.get(k));
                        H.set(k, 1, (constellation.getRxPos().getY() - satPosMat.get(k, 1)) / distPred.get(k));
                        H.set(k, 2, (constellation.getRxPos().getZ() - satPosMat.get(k, 2)) / distPred.get(k));
                        H.set(k, 3, 1.0);
                    }

                    // Compute the prefit vector (z)
                    z.set(prVect.minus(measPred));

                    // Estimate the unknowns (dxHat)
                    SimpleMatrix <Matrix> Cov       = H.transpose().mult(W).mult(H);
                    SimpleMatrix <Matrix> CovDOP    = H.transpose().mult(H);
                    SimpleMatrix <Matrix> dopMatrix = CovDOP.invert();
                    xHat.set(Cov.invert().mult(H.transpose()).mult(W).mult(z));
                    PDOP.set(Math.Sqrt(dopMatrix.get(0, 0) + dopMatrix.get(1, 1)));

                    // Update the receiver position
                    // rxPosSimpleVector.set(rxPosSimpleVector.plus(xHat));
                    rxPosSimpleVector.set(0, rxPosSimpleVector.get(0) + xHat.get(0));
                    rxPosSimpleVector.set(1, rxPosSimpleVector.get(1) + xHat.get(1));
                    rxPosSimpleVector.set(2, rxPosSimpleVector.get(2) + xHat.get(2));
                    rxPosSimpleVector.set(3, xHat.get(3));
                }

                clockBias = rxPosSimpleVector.get(3);
            }
            catch (Exception e)
            {
                //if (e.getClass() == IndexOutOfBoundsException.class){
                //    Log.e(TAG, "calculatePose: Satellites cleared before calculating result!");
                //} else if (e.getClass() == SingularMatrixException.class) {
                //    Log.e(TAG, "calculatePose: SingularMatrixException caught!");
                //}
                constellation.setRxPos(ZERO_POSE); // Right at the edge of the plot
                rxPosSimpleVector = Constellation.getRxPosAsVector(constellation.getRxPos());
                //e.printStackTrace();
            }

            //System.out.println();

            // Print the estimated receiver position
            //        rxPosSimpleVector.print();

            Log.Debug(TAG, "calculatePose: rxPosSimpleVector (ECEF): " + rxPosSimpleVector.get(0) + ", " + rxPosSimpleVector.get(1) + ", " + rxPosSimpleVector.get(2) + ";");

            Coordinates <Matrix> pose = Coordinates <Matrix> .globalXYZInstance(rxPosSimpleVector.get(0), rxPosSimpleVector.get(1), rxPosSimpleVector.get(2));

            Log.Debug(TAG, "calculatePose: pose (ECEF): " + pose.getX() + ", " + pose.getY() + ", " + pose.getZ() + ";");
            Log.Debug(TAG, "calculatePose: pose (lat-lon): " + pose.getGeodeticLatitude() + ", " + pose.getGeodeticLongitude() + ", " + pose.getGeodeticHeight() + ";");
            Log.Debug(TAG, "calculated PDOP:" + PDOP.get(0) + ";");

            return(pose);
        }
Example #3
0
        public override void calculateCorrection(Time currentTime, Coordinates <Matrix> approximatedPose, SatellitePosition satelliteCoordinates, NavigationProducer navigationProducer, Location initialLocation)
        {
            // Compute also the geodetic version of the user position (latitude, longitude, height)
            approximatedPose.computeGeodetic();

            // Get the user's height
            double height = approximatedPose.getGeodeticHeight();

            // Compute the elevation and azimuth angles for each satellite
            TopocentricCoordinates <Matrix> topo = new TopocentricCoordinates <Matrix>();

            topo.computeTopocentric(approximatedPose, satelliteCoordinates);

            // Assign the elevation information to a new variable
            double elevation = topo.getElevation();

            double tropoCorr = 0;

            if (height > 5000)
            {
                return;
            }

            elevation = Math.Abs(elevation) / 180.0 * Math.PI;
            if (elevation == 0)
            {
                elevation = elevation + 0.01;
            }

            // Numerical constants and tables for Saastamoinen algorithm
            // (troposphere correction)
            double hr = 50.0;

            int[]    ha = { 0, 500, 1000, 1500, 2000, 2500, 3000, 4000, 5000 };
            double[] ba = { 1.156, 1.079, 1.006, 0.938, 0.874, 0.813, 0.757, 0.654, 0.563 };

            // Saastamoinen algorithm
            double P = Constants.STANDARD_PRESSURE * Math.Pow((1 - 0.0000226 * height), 5.225);
            double T = Constants.STANDARD_TEMPERATURE - 0.0065 * height;
            double H = hr * Math.Exp(-0.0006396 * height);

            // If height is below zero, keep the maximum correction value
            double B = ba[0];

            // Otherwise, interpolate the tables
            if (height >= 0)
            {
                int i = 1;
                while (height > ha[i])
                {
                    i++;
                }
                double m = (ba[i] - ba[i - 1]) / (ha[i] - ha[i - 1]);
                B = ba[i - 1] + m * (height - ha[i - 1]);
            }

            double e = 0.01
                       * H
                       * Math.Exp(-37.2465 + 0.213166 * T - 0.000256908
                                  * Math.Pow(T, 2));

            tropoCorr = ((0.002277 / Math.Sin(elevation))
                         * (P - (B / Math.Pow(Math.Tan(elevation), 2))) + (0.002277 / Math.Sin(elevation))
                         * (1255 / T + 0.05) * e);

            correctionValue = tropoCorr;
        }