1 /*
2 * Copyright (C) 2019 Alberto Irurueta Carro (alberto@irurueta.com)
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 com.irurueta.navigation.gnss;
17
18 import com.irurueta.navigation.geodesic.Constants;
19
20 import java.util.ArrayList;
21 import java.util.Collection;
22
23 /**
24 * Computes satellites positions and velocities.
25 * This implementation is based on the equations defined in "Principles of GNSS, Inertial, and Multi-sensor
26 * Integrated Navigation Systems, Second Edition" and on the companion software available at:
27 * <a href="https://github.com/ymjdz/MATLAB-Codes/blob/master/Satellite_positions_and_velocities.m">
28 * https://github.com/ymjdz/MATLAB-Codes/blob/master/Satellite_positions_and_velocities.m
29 * </a>
30 */
31 public class SatelliteECEFPositionAndVelocityGenerator {
32
33 /**
34 * WGS84 Earth gravitational constant expressed in m^3 * s^-2
35 */
36 public static final double EARTH_GRAVITATIONAL_CONSTANT = Constants.EARTH_GRAVITATIONAL_CONSTANT;
37
38 /**
39 * Earth rotation rate expressed in radians per second (rad/s).
40 */
41 public static final double EARTH_ROTATION_RATE = Constants.EARTH_ROTATION_RATE;
42
43 /**
44 * Constructor.
45 * Prevents instantiation of utility class.
46 */
47 private SatelliteECEFPositionAndVelocityGenerator() {
48 }
49
50 /**
51 * Generates positions and velocities of satellites based on provided configuration.
52 *
53 * @param time current time expressed in seconds (s).
54 * @param config GNSS configuration.
55 * @return collection containing position and velocities of satellites.
56 */
57 public static Collection<ECEFPositionAndVelocity> generateSatellitesPositionAndVelocity(
58 final double time, final GNSSConfig config) {
59 final var result = new ArrayList<ECEFPositionAndVelocity>();
60 generateSatellitesPositionAndVelocity(time, config, result);
61 return result;
62 }
63
64 /**
65 * Generates positions and velocities of satellites based on provided configuration.
66 *
67 * @param time current time expressed in seconds (s).
68 * @param config GNSS configuration.
69 * @param result instance where computed positions and velocities of satellites will be stored.
70 */
71 public static void generateSatellitesPositionAndVelocity(
72 final double time, final GNSSConfig config, final Collection<ECEFPositionAndVelocity> result) {
73 result.clear();
74
75 final var numSatellites = config.getNumberOfSatellites();
76 for (var j = 0; j < numSatellites; j++) {
77 final var satellitePositionAndVelocity = new ECEFPositionAndVelocity();
78 generateSatellitePositionAndVelocity(time, config, j, satellitePositionAndVelocity);
79 result.add(satellitePositionAndVelocity);
80 }
81 }
82
83 /**
84 * Generates position and velocity of a single satellite based on provided configuration.
85 *
86 * @param time current time expressed in seconds (s).
87 * @param config GNSS configuration.
88 * @param j number of satellite whose position and velocity must be computed.
89 * @return computed satellite position and velocity.
90 */
91 public static ECEFPositionAndVelocity generateSatellitePositionAndVelocity(final double time,
92 final GNSSConfig config,
93 final int j) {
94 final var result = new ECEFPositionAndVelocity();
95 generateSatellitePositionAndVelocity(time, config, j, result);
96 return result;
97 }
98
99 /**
100 * Generates position and velocity of a single satellite based on provided configuration.
101 *
102 * @param time current time expressed in seconds (s).
103 * @param config GNSS configuration.
104 * @param j number of satellite whose position and velocity must be computed.
105 * @param result instance where computed satellite position and velocity will be stored.
106 */
107 public static void generateSatellitePositionAndVelocity(final double time, final GNSSConfig config,
108 final int j, final ECEFPositionAndVelocity result) {
109
110 // Convert inclination angle to radians.
111 final var inclinationRadians = Math.toRadians(config.getSatellitesInclinationDegrees());
112
113 // Determine orbital angular rate using (8.8)
114 final var orbitalRadius = config.getOrbitalRadiusOfSatellites();
115 final var orbitalRadius3 = orbitalRadius * orbitalRadius * orbitalRadius;
116 final var omegaIs = Math.sqrt(EARTH_GRAVITATIONAL_CONSTANT / orbitalRadius3);
117
118 // determine constellation time
119 final var constTime = time + config.getConstellationTimingOffset();
120
121 // (Corrected) argument of latitude
122 final var uOsO = 2.0 * Math.PI * j / config.getNumberOfSatellites() + omegaIs * constTime;
123
124 // Satellite position in the orbital frame from (8.14)
125 final var cosUoso = Math.cos(uOsO);
126 final var sinUoso = Math.sin(uOsO);
127 final var rOsO1 = orbitalRadius * cosUoso;
128 final var rOsO2 = orbitalRadius * sinUoso;
129
130 // longitude of the ascending node from (8.16)
131 final var constDeltaLambdaRadians = Math.toRadians(config.getConstellationLongitudeOffsetDegrees());
132 final var omega = (Math.PI * ((j + 1) % 6) / 3.0 + constDeltaLambdaRadians) - EARTH_ROTATION_RATE * constTime;
133
134 // ECEF satellite position from (8.19)
135 final var cosOmega = Math.cos(omega);
136 final var sinOmega = Math.sin(omega);
137 final var cosInclination = Math.cos(inclinationRadians);
138 final var sinInclination = Math.sin(inclinationRadians);
139
140 final var satelliteX = rOsO1 * cosOmega - rOsO2 * cosInclination * sinOmega;
141 final var satelliteY = rOsO1 * sinOmega + rOsO2 * cosInclination * cosOmega;
142 final var satelliteZ = rOsO2 * sinInclination;
143
144 // Satellite velocity in the orbital frame from (8.25), noting that with a circular orbit rOsO is
145 // constant and the time derivative of uOsO is omegaIs.
146 final var tmp = orbitalRadius * omegaIs;
147 final var vOsO1 = -tmp * sinUoso;
148 final var vOsO2 = tmp * cosUoso;
149
150 // ECEF satellite velocity from (8.26)
151 final var satelliteVx = vOsO1 * cosOmega - vOsO2 * cosInclination * sinOmega + EARTH_ROTATION_RATE * satelliteY;
152 final var satelliteVy = vOsO1 * sinOmega + vOsO2 * cosInclination * cosOmega - EARTH_ROTATION_RATE * satelliteX;
153 final var satelliteVz = vOsO2 * sinInclination;
154
155 result.setPositionCoordinates(satelliteX, satelliteY, satelliteZ);
156 result.setVelocityCoordinates(satelliteVx, satelliteVy, satelliteVz);
157 }
158 }