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 package android.location.cts.gnss.pseudorange; 17 import android.location.cts.gnss.nano.Ephemeris.GpsEphemerisProto; 18 /** 19 * Calculate the GPS satellite clock correction based on parameters observed from the navigation 20 * message 21 * <p>Source: Page 88 - 90 of the ICD-GPS 200 22 */ 23 public class SatelliteClockCorrectionCalculator { 24 private static final double SPEED_OF_LIGHT_MPS = 299792458.0; 25 private static final double EARTH_UNIVERSAL_GRAVITATIONAL_CONSTANT_M3_SM2 = 3.986005e14; 26 private static final double RELATIVISTIC_CONSTANT_F = -4.442807633e-10; 27 private static final int SECONDS_IN_WEEK = 604800; 28 private static final double ACCURACY_TOLERANCE = 1.0e-11; 29 private static final int MAX_ITERATIONS = 100; 30 /** 31 * Compute the GPS satellite clock correction term in meters iteratively following page 88 - 90 32 * and 98 - 100 of the ICD GPS 200. The method returns a pair of satellite clock correction in 33 * meters and Kepler Eccentric Anomaly in Radians. 34 * 35 * @param ephemerisProto parameters of the navigation message 36 * @param receiverGpsTowAtTimeOfTransmission Reciever estimate of GPS time of week when signal was 37 * transmitted (seconds) 38 * @param receiverGpsWeekAtTimeOfTrasnmission Receiver estimate of GPS week when signal was 39 * transmitted (0-1024+) 40 * @throws Exception 41 */ calculateSatClockCorrAndEccAnomAndTkIteratively( GpsEphemerisProto ephemerisProto, double receiverGpsTowAtTimeOfTransmission, double receiverGpsWeekAtTimeOfTrasnmission)42 public static SatClockCorrection calculateSatClockCorrAndEccAnomAndTkIteratively( 43 GpsEphemerisProto ephemerisProto, double receiverGpsTowAtTimeOfTransmission, 44 double receiverGpsWeekAtTimeOfTrasnmission) throws Exception { 45 // Units are not added in the variable names to have the same name as the ICD-GPS200 46 // Mean anomaly (radians) 47 double meanAnomalyRad; 48 // Kepler's Equation for Eccentric Anomaly iteratively (Radians) 49 double eccentricAnomalyRad; 50 // Semi-major axis of orbit (meters) 51 double a = ephemerisProto.rootOfA * ephemerisProto.rootOfA; 52 // Computed mean motion (radians/seconds) 53 double n0 = Math.sqrt(EARTH_UNIVERSAL_GRAVITATIONAL_CONSTANT_M3_SM2 / (a * a * a)); 54 // Corrected mean motion (radians/seconds) 55 double n = n0 + ephemerisProto.deltaN; 56 // In the following, Receiver GPS week and ephemeris GPS week are used to correct for week 57 // rollover when calculating the time from clock reference epoch (tcSec) 58 double timeOfTransmissionIncludingRxWeekSec = 59 receiverGpsWeekAtTimeOfTrasnmission * SECONDS_IN_WEEK + receiverGpsTowAtTimeOfTransmission; 60 // time from clock reference epoch (seconds) page 88 ICD-GPS200 61 double tcSec = timeOfTransmissionIncludingRxWeekSec 62 - (ephemerisProto.week * SECONDS_IN_WEEK + ephemerisProto.toc); 63 // Correction for week rollover 64 tcSec = fixWeekRollover(tcSec); 65 double oldEcentricAnomalyRad = 0.0; 66 double newSatClockCorrectionSeconds = 0.0; 67 double relativisticCorrection = 0.0; 68 double changeInSatClockCorrection = 0.0; 69 // Initial satellite clock correction (unknown relativistic correction). Iterate to correct 70 // with the relativistic effect and obtain a stable 71 final double initSatClockCorrectionSeconds = ephemerisProto.af0 72 + ephemerisProto.af1 * tcSec 73 + ephemerisProto.af2 * tcSec * tcSec - ephemerisProto.tgd; 74 double satClockCorrectionSeconds = initSatClockCorrectionSeconds; 75 double tkSec; 76 int satClockCorrectionsCounter = 0; 77 do { 78 int eccentricAnomalyCounter = 0; 79 // time from ephemeris reference epoch (seconds) page 98 ICD-GPS200 80 tkSec = timeOfTransmissionIncludingRxWeekSec - ( 81 ephemerisProto.week * SECONDS_IN_WEEK + ephemerisProto.toe 82 + satClockCorrectionSeconds); 83 // Correction for week rollover 84 tkSec = fixWeekRollover(tkSec); 85 // Mean anomaly (radians) 86 meanAnomalyRad = ephemerisProto.m0 + n * tkSec; 87 // eccentric anomaly (radians) 88 eccentricAnomalyRad = meanAnomalyRad; 89 // Iteratively solve for Kepler's eccentric anomaly according to ICD-GPS200 page 99 90 do { 91 oldEcentricAnomalyRad = eccentricAnomalyRad; 92 eccentricAnomalyRad = 93 meanAnomalyRad + ephemerisProto.e * Math.sin(eccentricAnomalyRad); 94 eccentricAnomalyCounter++; 95 if (eccentricAnomalyCounter > MAX_ITERATIONS) { 96 throw new Exception("Kepler Eccentric Anomaly calculation did not converge in " 97 + MAX_ITERATIONS + " iterations"); 98 } 99 } while (Math.abs(oldEcentricAnomalyRad - eccentricAnomalyRad) > ACCURACY_TOLERANCE); 100 // relativistic correction term (seconds) 101 relativisticCorrection = RELATIVISTIC_CONSTANT_F * ephemerisProto.e 102 * ephemerisProto.rootOfA * Math.sin(eccentricAnomalyRad); 103 // satellite clock correction including relativistic effect 104 newSatClockCorrectionSeconds = initSatClockCorrectionSeconds + relativisticCorrection; 105 changeInSatClockCorrection = 106 Math.abs(satClockCorrectionSeconds - newSatClockCorrectionSeconds); 107 satClockCorrectionSeconds = newSatClockCorrectionSeconds; 108 satClockCorrectionsCounter++; 109 if (satClockCorrectionsCounter > MAX_ITERATIONS) { 110 throw new Exception("Satellite Clock Correction calculation did not converge in " 111 + MAX_ITERATIONS + " iterations"); 112 } 113 } while (changeInSatClockCorrection > ACCURACY_TOLERANCE); 114 tkSec = timeOfTransmissionIncludingRxWeekSec - ( 115 ephemerisProto.week * SECONDS_IN_WEEK + ephemerisProto.toe 116 + satClockCorrectionSeconds); 117 // return satellite clock correction (meters) and Kepler Eccentric Anomaly in Radians 118 return new SatClockCorrection(satClockCorrectionSeconds * SPEED_OF_LIGHT_MPS, 119 eccentricAnomalyRad, tkSec); 120 } 121 122 /** 123 * Calculates Satellite Clock Error Rate in (meters/second) by subtracting the Satellite 124 * Clock Error Values at t+0.5s and t-0.5s. 125 * 126 * <p>This approximation is more accurate than differentiating because both the orbital 127 * and relativity terms have non-linearities that are not easily differentiable. 128 */ calculateSatClockCorrErrorRate( GpsEphemerisProto ephemerisProto, double receiverGpsTowAtTimeOfTransmissionSeconds, double receiverGpsWeekAtTimeOfTrasnmission)129 public static double calculateSatClockCorrErrorRate( 130 GpsEphemerisProto ephemerisProto, double receiverGpsTowAtTimeOfTransmissionSeconds, 131 double receiverGpsWeekAtTimeOfTrasnmission) throws Exception { 132 SatClockCorrection satClockCorrectionPlus = calculateSatClockCorrAndEccAnomAndTkIteratively( 133 ephemerisProto, receiverGpsTowAtTimeOfTransmissionSeconds + 0.5, 134 receiverGpsWeekAtTimeOfTrasnmission); 135 SatClockCorrection satClockCorrectionMinus = calculateSatClockCorrAndEccAnomAndTkIteratively( 136 ephemerisProto, receiverGpsTowAtTimeOfTransmissionSeconds - 0.5, 137 receiverGpsWeekAtTimeOfTrasnmission); 138 double satelliteClockErrorRate = satClockCorrectionPlus.satelliteClockCorrectionMeters 139 - satClockCorrectionMinus.satelliteClockCorrectionMeters; 140 return satelliteClockErrorRate; 141 } 142 143 /** 144 * Method to check for week rollover according to ICD-GPS 200 page 98. 145 * 146 * <p>Result should be between -302400 and 302400 if the ephemeris is within one week of 147 * transmission, otherwise it is adjusted to the correct range 148 */ fixWeekRollover(double time)149 private static double fixWeekRollover(double time) { 150 double correctedTime = time; 151 if (time > SECONDS_IN_WEEK / 2.0) { 152 correctedTime = time - SECONDS_IN_WEEK; 153 } 154 if (time < -SECONDS_IN_WEEK / 2.0) { 155 correctedTime = time + SECONDS_IN_WEEK; 156 } 157 return correctedTime; 158 } 159 /** 160 * 161 * Class containing the satellite clock correction parameters: The satellite clock correction in 162 * meters, Kepler Eccentric Anomaly in Radians and the time from the reference epoch in seconds. 163 */ 164 public static class SatClockCorrection { 165 /** 166 * Satellite clock correction in meters 167 */ 168 public final double satelliteClockCorrectionMeters; 169 /** 170 * Satellite clock correction in meters 171 */ 172 public final double satelliteClockRateCorrectionMetersPerSecond; 173 /** 174 * Kepler Eccentric Anomaly in Radians 175 */ 176 public final double eccentricAnomalyRadians; 177 /** 178 * Time from the reference epoch in Seconds 179 */ 180 public final double timeFromRefEpochSec; 181 /** 182 * Constructor 183 */ SatClockCorrection(double satelliteClockCorrectionMeters, double eccentricAnomalyRadians, double timeFromRefEpochSec)184 public SatClockCorrection(double satelliteClockCorrectionMeters, double eccentricAnomalyRadians, 185 double timeFromRefEpochSec) { 186 this.satelliteClockCorrectionMeters = satelliteClockCorrectionMeters; 187 this.eccentricAnomalyRadians = eccentricAnomalyRadians; 188 this.timeFromRefEpochSec = timeFromRefEpochSec; 189 this.satelliteClockRateCorrectionMetersPerSecond = Double.NaN; 190 } 191 /** 192 * Alternative Constructor 193 */ SatClockCorrection(double satelliteClockCorrectionMeters, double satelliteClockRateCorrectionMeters, double eccentricAnomalyRadians, double timeFromRefEpochSec)194 public SatClockCorrection(double satelliteClockCorrectionMeters, 195 double satelliteClockRateCorrectionMeters, double eccentricAnomalyRadians, 196 double timeFromRefEpochSec){ 197 this.satelliteClockCorrectionMeters = satelliteClockCorrectionMeters; 198 this.eccentricAnomalyRadians = eccentricAnomalyRadians; 199 this.timeFromRefEpochSec = timeFromRefEpochSec; 200 this.satelliteClockRateCorrectionMetersPerSecond = satelliteClockRateCorrectionMeters; 201 } 202 } 203 } 204