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.gnss.pseudorange; 18 19 import android.location.cts.gnss.pseudorange.SatelliteClockCorrectionCalculator.SatClockCorrection; 20 import android.location.cts.gnss.nano.Ephemeris.GpsEphemerisProto; 21 22 /* Class to calculate GPS satellite positions from the ephemeris data */ 23 public class SatellitePositionCalculator { 24 private static final double SPEED_OF_LIGHT_MPS = 299792458.0; 25 private static final double UNIVERSAL_GRAVITATIONAL_PARAMETER_M3_SM2 = 3.986005e14; 26 private static final int NUMBER_OF_ITERATIONS_FOR_SAT_POS_CALCULATION = 5; 27 private static final double EARTH_ROTATION_RATE_RAD_PER_SEC = 7.2921151467e-5; 28 29 /** 30 * 31 * Calculate GPS satellite position and velocity from ephemeris including the Sagnac effect 32 * starting from unknown user to satellite distance and speed. So we start from an initial guess 33 * of the user to satellite range and range rate and iterate to include the Sagnac effect. Few 34 * iterations are enough to achieve a satellite position with millimeter accuracy. 35 * A {@code PositionAndVelocity} class is returned containing satellite position in meters 36 * (x, y and z) and velocity in meters per second (x, y, z) 37 * 38 * <p>Satelite position and velocity equations are obtained from: 39 * http://www.gps.gov/technical/icwg/ICD-GPS-200C.pdf) pages 94 - 101 and 40 * http://fenrir.naruoka.org/download/autopilot/note/080205_gps/gps_velocity.pdf 41 * 42 * @param ephemerisProto parameters of the navigation message 43 * @param receiverGpsTowAtTimeOfTransmissionCorrectedSec Receiver estimate of GPS time of week 44 * when signal was transmitted corrected with the satellite clock drift (seconds) 45 * @param receiverGpsWeekAtTimeOfTransmission Receiver estimate of GPS week when signal was 46 * transmitted (0-1024+) 47 * @param userPosXMeters Last known user x-position (if known) [meters] 48 * @param userPosYMeters Last known user y-position (if known) [meters] 49 * @param userPosZMeters Last known user z-position (if known) [meters] 50 * @throws Exception 51 */ calculateSatellitePositionAndVelocityFromEphemeris(GpsEphemerisProto ephemerisProto, double receiverGpsTowAtTimeOfTransmissionCorrectedSec, int receiverGpsWeekAtTimeOfTransmission, double userPosXMeters, double userPosYMeters, double userPosZMeters)52 public static PositionAndVelocity calculateSatellitePositionAndVelocityFromEphemeris 53 (GpsEphemerisProto ephemerisProto, double receiverGpsTowAtTimeOfTransmissionCorrectedSec, 54 int receiverGpsWeekAtTimeOfTransmission, 55 double userPosXMeters, 56 double userPosYMeters, 57 double userPosZMeters) throws Exception { 58 59 // lets start with a first user to sat distance guess of 70 ms and zero velocity 60 RangeAndRangeRate userSatRangeAndRate = new RangeAndRangeRate 61 (0.070 * SPEED_OF_LIGHT_MPS, 0.0 /* range rate*/); 62 63 // To apply sagnac effect correction, We are starting from an approximate guess of the user to 64 // satellite range, iterate 3 times and that should be enough to reach millimeter accuracy 65 PositionAndVelocity satPosAndVel = new PositionAndVelocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); 66 PositionAndVelocity userPosAndVel = 67 new PositionAndVelocity(userPosXMeters, userPosYMeters, userPosZMeters, 68 0.0 /* user velocity x*/, 0.0 /* user velocity y*/, 0.0 /* user velocity z */); 69 for (int i = 0; i < NUMBER_OF_ITERATIONS_FOR_SAT_POS_CALCULATION; i++) { 70 calculateSatellitePositionAndVelocity(ephemerisProto, 71 receiverGpsTowAtTimeOfTransmissionCorrectedSec, receiverGpsWeekAtTimeOfTransmission, 72 userSatRangeAndRate, satPosAndVel); 73 computeUserToSatelliteRangeAndRangeRate(userPosAndVel, satPosAndVel, userSatRangeAndRate); 74 } 75 return satPosAndVel; 76 } 77 78 /** 79 * Calculate GPS satellite position and velocity from ephemeris based on the ICD-GPS-200. 80 * Satellite position in meters (x, y and z) and velocity in meters per second (x, y, z) are set 81 * in the passed {@code PositionAndVelocity} instance. 82 * 83 * <p>Sources: http://www.gps.gov/technical/icwg/ICD-GPS-200C.pdf) pages 94 - 101 and 84 * http://fenrir.naruoka.org/download/autopilot/note/080205_gps/gps_velocity.pdf 85 * 86 * @param ephemerisProto parameters of the navigation message 87 * @param receiverGpsTowAtTimeOfTransmissionCorrected Receiver estimate of GPS time of week when 88 * signal was transmitted corrected with the satellite clock drift (seconds) 89 * @param receiverGpsWeekAtTimeOfTransmission Receiver estimate of GPS week when signal was 90 * transmitted (0-1024+) 91 * @param userSatRangeAndRate user to satellite range and range rate 92 * @param satPosAndVel Satellite position and velocity instance in which the method results will 93 * be set 94 * @throws Exception 95 */ calculateSatellitePositionAndVelocity(GpsEphemerisProto ephemerisProto, double receiverGpsTowAtTimeOfTransmissionCorrected, int receiverGpsWeekAtTimeOfTransmission, RangeAndRangeRate userSatRangeAndRate, PositionAndVelocity satPosAndVel)96 public static void calculateSatellitePositionAndVelocity(GpsEphemerisProto ephemerisProto, 97 double receiverGpsTowAtTimeOfTransmissionCorrected, int receiverGpsWeekAtTimeOfTransmission, 98 RangeAndRangeRate userSatRangeAndRate, PositionAndVelocity satPosAndVel) throws Exception { 99 100 // Calculate satellite clock correction (meters), Kepler Eccentric anomaly (radians) and time 101 // from ephemeris refrence epoch (tkSec) iteratively 102 SatClockCorrection satClockCorrectionValues = 103 SatelliteClockCorrectionCalculator.calculateSatClockCorrAndEccAnomAndTkIteratively( 104 ephemerisProto, receiverGpsTowAtTimeOfTransmissionCorrected, 105 receiverGpsWeekAtTimeOfTransmission); 106 107 double eccentricAnomalyRadians = satClockCorrectionValues.eccentricAnomalyRadians; 108 double tkSec = satClockCorrectionValues.timeFromRefEpochSec; 109 110 // True_anomaly (angle from perigee) 111 double trueAnomalyRadians = Math.atan2( 112 Math.sqrt(1.0 - ephemerisProto.e * ephemerisProto.e) 113 * Math.sin(eccentricAnomalyRadians), 114 Math.cos(eccentricAnomalyRadians) - ephemerisProto.e); 115 116 // Argument of latitude of the satellite 117 double argumentOfLatitudeRadians = trueAnomalyRadians + ephemerisProto.omega; 118 119 // Radius of satellite orbit 120 double radiusOfSatelliteOrbitMeters = ephemerisProto.rootOfA * ephemerisProto.rootOfA 121 * (1.0 - ephemerisProto.e * Math.cos(eccentricAnomalyRadians)); 122 123 // Radius correction due to second harmonic perturbations of the orbit 124 double radiusCorrectionMeters = ephemerisProto.crc 125 * Math.cos(2.0 * argumentOfLatitudeRadians) + ephemerisProto.crs 126 * Math.sin(2.0 * argumentOfLatitudeRadians); 127 // Argument of latitude correction due to second harmonic perturbations of the orbit 128 double argumentOfLatitudeCorrectionRadians = ephemerisProto.cuc 129 * Math.cos(2.0 * argumentOfLatitudeRadians) + ephemerisProto.cus 130 * Math.sin(2.0 * argumentOfLatitudeRadians); 131 // Correction to inclination due to second harmonic perturbations of the orbit 132 double inclinationCorrectionRadians = ephemerisProto.cic 133 * Math.cos(2.0 * argumentOfLatitudeRadians) + ephemerisProto.cis 134 * Math.sin(2.0 * argumentOfLatitudeRadians); 135 136 // Corrected radius of satellite orbit 137 radiusOfSatelliteOrbitMeters += radiusCorrectionMeters; 138 // Corrected argument of latitude 139 argumentOfLatitudeRadians += argumentOfLatitudeCorrectionRadians; 140 // Corrected inclination 141 double inclinationRadians = 142 ephemerisProto.i0 + inclinationCorrectionRadians + ephemerisProto.iDot * tkSec; 143 144 // Position in orbital plane 145 double xPositionMeters = radiusOfSatelliteOrbitMeters * Math.cos(argumentOfLatitudeRadians); 146 double yPositionMeters = radiusOfSatelliteOrbitMeters * Math.sin(argumentOfLatitudeRadians); 147 148 // Corrected longitude of the ascending node (signal propagation time is included to compensate 149 // for the Sagnac effect) 150 double omegaKRadians = ephemerisProto.omega0 151 + (ephemerisProto.omegaDot - EARTH_ROTATION_RATE_RAD_PER_SEC) * tkSec 152 - EARTH_ROTATION_RATE_RAD_PER_SEC 153 * (ephemerisProto.toe + userSatRangeAndRate.rangeMeters / SPEED_OF_LIGHT_MPS); 154 155 // compute the resulting satellite position 156 double satPosXMeters = xPositionMeters * Math.cos(omegaKRadians) - yPositionMeters 157 * Math.cos(inclinationRadians) * Math.sin(omegaKRadians); 158 double satPosYMeters = xPositionMeters * Math.sin(omegaKRadians) + yPositionMeters 159 * Math.cos(inclinationRadians) * Math.cos(omegaKRadians); 160 double satPosZMeters = yPositionMeters * Math.sin(inclinationRadians); 161 162 // Satellite Velocity Computation using the broadcast ephemeris 163 // http://fenrir.naruoka.org/download/autopilot/note/080205_gps/gps_velocity.pdf 164 // Units are not added in some of the variable names to have the same name as the ICD-GPS200 165 // Semi-major axis of orbit (meters) 166 double a = ephemerisProto.rootOfA * ephemerisProto.rootOfA; 167 // Computed mean motion (radians/seconds) 168 double n0 = Math.sqrt(UNIVERSAL_GRAVITATIONAL_PARAMETER_M3_SM2 / (a * a * a)); 169 // Corrected mean motion (radians/seconds) 170 double n = n0 + ephemerisProto.deltaN; 171 // Derivative of mean anomaly (radians/seconds) 172 double meanAnomalyDotRadPerSec = n; 173 // Derivative of eccentric anomaly (radians/seconds) 174 double eccentricAnomalyDotRadPerSec = 175 meanAnomalyDotRadPerSec / (1.0 - ephemerisProto.e * Math.cos(eccentricAnomalyRadians)); 176 // Derivative of true anomaly (radians/seconds) 177 double trueAnomalydotRadPerSec = Math.sin(eccentricAnomalyRadians) 178 * eccentricAnomalyDotRadPerSec 179 * (1.0 + ephemerisProto.e * Math.cos(trueAnomalyRadians)) / ( 180 Math.sin(trueAnomalyRadians) 181 * (1.0 - ephemerisProto.e * Math.cos(eccentricAnomalyRadians))); 182 // Derivative of argument of latitude (radians/seconds) 183 double argumentOfLatitudeDotRadPerSec = trueAnomalydotRadPerSec + 2.0 * (ephemerisProto.cus 184 * Math.cos(2.0 * argumentOfLatitudeRadians) - ephemerisProto.cuc 185 * Math.sin(2.0 * argumentOfLatitudeRadians)) * trueAnomalydotRadPerSec; 186 // Derivative of radius of satellite orbit (m/s) 187 double radiusOfSatelliteOrbitDotMPerSec = a * ephemerisProto.e 188 * Math.sin(eccentricAnomalyRadians) * n 189 / (1.0 - ephemerisProto.e * Math.cos(eccentricAnomalyRadians)) + 2.0 * ( 190 ephemerisProto.crs * Math.cos(2.0 * argumentOfLatitudeRadians) 191 - ephemerisProto.crc * Math.sin(2.0 * argumentOfLatitudeRadians)) 192 * trueAnomalydotRadPerSec; 193 // Derivative of the inclination (radians/seconds) 194 double inclinationDotRadPerSec = ephemerisProto.iDot + (ephemerisProto.cis 195 * Math.cos(2.0 * argumentOfLatitudeRadians) - ephemerisProto.cic 196 * Math.sin(2.0 * argumentOfLatitudeRadians)) * 2.0 * trueAnomalydotRadPerSec; 197 198 double xVelocityMPS = radiusOfSatelliteOrbitDotMPerSec * Math.cos(argumentOfLatitudeRadians) 199 - yPositionMeters * argumentOfLatitudeDotRadPerSec; 200 double yVelocityMPS = radiusOfSatelliteOrbitDotMPerSec * Math.sin(argumentOfLatitudeRadians) 201 + xPositionMeters * argumentOfLatitudeDotRadPerSec; 202 203 // Corrected rate of right ascension including compensation for the Sagnac effect 204 double omegaDotRadPerSec = ephemerisProto.omegaDot - EARTH_ROTATION_RATE_RAD_PER_SEC 205 * (1.0 + userSatRangeAndRate.rangeRateMetersPerSec / SPEED_OF_LIGHT_MPS); 206 // compute the resulting satellite velocity 207 double satVelXMPS = 208 (xVelocityMPS - yPositionMeters * Math.cos(inclinationRadians) * omegaDotRadPerSec) 209 * Math.cos(omegaKRadians) - (xPositionMeters * omegaDotRadPerSec + yVelocityMPS 210 * Math.cos(inclinationRadians) - yPositionMeters * Math.sin(inclinationRadians) 211 * inclinationDotRadPerSec) * Math.sin(omegaKRadians); 212 double satVelYMPS = 213 (xVelocityMPS - yPositionMeters * Math.cos(inclinationRadians) * omegaDotRadPerSec) 214 * Math.sin(omegaKRadians) + (xPositionMeters * omegaDotRadPerSec + yVelocityMPS 215 * Math.cos(inclinationRadians) - yPositionMeters * Math.sin(inclinationRadians) 216 * inclinationDotRadPerSec) * Math.cos(omegaKRadians); 217 double satVelZMPS = yVelocityMPS * Math.sin(inclinationRadians) + yPositionMeters 218 * Math.cos(inclinationRadians) * inclinationDotRadPerSec; 219 220 satPosAndVel.positionXMeters = satPosXMeters; 221 satPosAndVel.positionYMeters = satPosYMeters; 222 satPosAndVel.positionZMeters = satPosZMeters; 223 satPosAndVel.velocityXMetersPerSec = satVelXMPS; 224 satPosAndVel.velocityYMetersPerSec = satVelYMPS; 225 satPosAndVel.velocityZMetersPerSec = satVelZMPS; 226 } 227 228 /** 229 * Compute and set the passed {@code RangeAndRangeRate} instance containing user to satellite 230 * range (meters) and range rate (m/s) given the user position (ECEF meters), user velocity (m/s), 231 * satellite position (ECEF meters) and satellite velocity (m/s). 232 */ computeUserToSatelliteRangeAndRangeRate(PositionAndVelocity userPosAndVel, PositionAndVelocity satPosAndVel, RangeAndRangeRate rangeAndRangeRate)233 private static void computeUserToSatelliteRangeAndRangeRate(PositionAndVelocity userPosAndVel, 234 PositionAndVelocity satPosAndVel, RangeAndRangeRate rangeAndRangeRate) { 235 double dXMeters = satPosAndVel.positionXMeters - userPosAndVel.positionXMeters; 236 double dYMeters = satPosAndVel.positionYMeters - userPosAndVel.positionYMeters; 237 double dZMeters = satPosAndVel.positionZMeters - userPosAndVel.positionZMeters; 238 // range in meters 239 double rangeMeters = Math.sqrt(dXMeters * dXMeters + dYMeters * dYMeters + dZMeters * dZMeters); 240 // range rate in meters / second 241 double rangeRateMetersPerSec = 242 ((userPosAndVel.velocityXMetersPerSec - satPosAndVel.velocityXMetersPerSec) * dXMeters 243 + (userPosAndVel.velocityYMetersPerSec - satPosAndVel.velocityYMetersPerSec) * dYMeters 244 + (userPosAndVel.velocityZMetersPerSec - satPosAndVel.velocityZMetersPerSec) * dZMeters) 245 / rangeMeters; 246 rangeAndRangeRate.rangeMeters = rangeMeters; 247 rangeAndRangeRate.rangeRateMetersPerSec = rangeRateMetersPerSec; 248 } 249 250 /** 251 * 252 * A class containing position values (x, y, z) in meters and velocity values (x, y, z) in meters 253 * per seconds 254 */ 255 public static class PositionAndVelocity { 256 /* x - position in meters */ 257 public double positionXMeters; 258 /* y - position in meters */ 259 public double positionYMeters; 260 /* z - position in meters */ 261 public double positionZMeters; 262 /* x - velocity in meters */ 263 public double velocityXMetersPerSec; 264 /* y - velocity in meters */ 265 public double velocityYMetersPerSec; 266 /* z - velocity in meters */ 267 public double velocityZMetersPerSec; 268 269 /* Constructor */ PositionAndVelocity(double positionXMeters, double positionYMeters, double positionZMeters, double velocityXMetersPerSec, double velocityYMetersPerSec, double velocityZMetersPerSec)270 public PositionAndVelocity(double positionXMeters, 271 double positionYMeters, 272 double positionZMeters, 273 double velocityXMetersPerSec, 274 double velocityYMetersPerSec, 275 double velocityZMetersPerSec) { 276 this.positionXMeters = positionXMeters; 277 this.positionYMeters = positionYMeters; 278 this.positionZMeters = positionZMeters; 279 this.velocityXMetersPerSec = velocityXMetersPerSec; 280 this.velocityYMetersPerSec = velocityYMetersPerSec; 281 this.velocityZMetersPerSec = velocityZMetersPerSec; 282 } 283 } 284 285 /* A class containing range of satellite to user in meters and range rate in meters per seconds */ 286 public static class RangeAndRangeRate { 287 /* Range in meters */ 288 public double rangeMeters; 289 /* Range rate in meters per seconds */ 290 public double rangeRateMetersPerSec; 291 292 /* Constructor */ RangeAndRangeRate(double rangeMeters, double rangeRateMetersPerSec)293 public RangeAndRangeRate(double rangeMeters, double rangeRateMetersPerSec) { 294 this.rangeMeters = rangeMeters; 295 this.rangeRateMetersPerSec = rangeRateMetersPerSec; 296 } 297 } 298 }