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.algebra.Matrix;
19 import com.irurueta.navigation.frames.CoordinateTransformation;
20 import com.irurueta.navigation.frames.ECEFPosition;
21 import com.irurueta.navigation.frames.NEDPosition;
22 import com.irurueta.navigation.frames.NEDVelocity;
23 import com.irurueta.navigation.frames.converters.ECEFtoNEDPositionVelocityConverter;
24
25 import java.util.ArrayList;
26 import java.util.List;
27 import java.util.Random;
28
29 /**
30 * Generates the GNSS range errors due to signal in space, ionosphere and troposphere
31 * errors based on the elevation angles.
32 * This implementation is based on the equations defined in "Principles of GNSS, Inertial, and Multi-sensor
33 * Integrated Navigation Systems, Second Edition" and on the companion software available at:
34 * <a href="https://github.com/ymjdz/MATLAB-Codes/blob/master/Initialize_GNSS_biases.m">
35 * https://github.com/ymjdz/MATLAB-Codes/blob/master/Initialize_GNSS_biases.m
36 * </a>
37 */
38 public class GNSSBiasesGenerator {
39
40 /**
41 * Ionosphere factor.
42 */
43 private static final double IONO_FACTOR = 0.899;
44
45 /**
46 * Troposphere factor.
47 */
48 private static final double TROPO_FACTOR = 0.998;
49
50 /**
51 * Constructor.
52 * Prevents instantiation of utility class.
53 */
54 private GNSSBiasesGenerator() {
55 }
56
57 /**
58 * Generates biases.
59 *
60 * @param satellitePositions ECEF satellite positions expressed in meters (m).
61 * @param userPosition ECEF user position expressed in meters (m).
62 * @param config GNSS configuration.
63 * @param random random number generator.
64 * @return list of generated biases for each provided satellite position.
65 */
66 public static List<Double> generateBiases(
67 final List<ECEFPosition> satellitePositions, final ECEFPosition userPosition, final GNSSConfig config,
68 final Random random) {
69 final var result = new ArrayList<Double>();
70 generateBiases(satellitePositions, userPosition, config, random, result);
71 return result;
72 }
73
74 /**
75 * Generates biases.
76 *
77 * @param satellitePositions ECEF satellite positions expressed in meters (m).
78 * @param userPosition ECEF user position expressed in meters (m).
79 * @param config GNSS configuration.
80 * @param random random number generator.
81 * @param result instance where generated biases for each
82 * provided satellite position will be stored.
83 */
84 public static void generateBiases(
85 final List<ECEFPosition> satellitePositions, final ECEFPosition userPosition, final GNSSConfig config,
86 final Random random, final List<Double> result) {
87 // Calculate NED user position
88 final var userNedPosition = new NEDPosition();
89 final var userNedVelocity = new NEDVelocity();
90 ECEFtoNEDPositionVelocityConverter.convertECEFtoNED(
91 userPosition.getX(), userPosition.getY(), userPosition.getZ(), 0.0, 0.0, 0.0,
92 userNedPosition, userNedVelocity);
93 generate(satellitePositions, userPosition, userNedPosition.getLatitude(), userNedPosition.getLongitude(),
94 config, random, result);
95 }
96
97 /**
98 * Generates biases.
99 *
100 * @param satellitePosition ECEF satellite positions expressed in meters (m).
101 * @param userPosition ECEF user position expressed in meters (m).
102 * @param config GNSS configuration.
103 * @param random random number generator.
104 * @return generated bias for provided satellite position.
105 */
106 public static double generateBias(final ECEFPosition satellitePosition, final ECEFPosition userPosition,
107 final GNSSConfig config, final Random random) {
108 // Calculate NED user position
109 final var userNedPosition = new NEDPosition();
110 final var userNedVelocity = new NEDVelocity();
111 ECEFtoNEDPositionVelocityConverter.convertECEFtoNED(
112 userPosition.getX(), userPosition.getY(), userPosition.getZ(), 0.0, 0.0, 0.0,
113 userNedPosition, userNedVelocity);
114 final var userLatitude = userNedPosition.getLatitude();
115 final var userLongitude = userNedPosition.getLongitude();
116
117 // Calculate ECEF to NED coordinate transformation matrix
118 final var cen = CoordinateTransformation.ecefToNedMatrix(userLatitude, userLongitude);
119 return generate(satellitePosition, userPosition, config, cen, random);
120 }
121
122 /**
123 * Generates biases.
124 *
125 * @param satellitePositions ECEF satellite positions expressed in meters (m).
126 * @param userPosition ECEF user position expressed in meters (m).
127 * @param userLatitude user latitude expressed in radians (rad).
128 * @param userLongitude user longitude expressed in radians (rad).
129 * @param config GNSS configuration.
130 * @param random random number generator.
131 * @param result instance where generated biases for each
132 * provided satellite position will be stored.
133 */
134 private static void generate(
135 final List<ECEFPosition> satellitePositions, final ECEFPosition userPosition,
136 final double userLatitude, final double userLongitude, final GNSSConfig config, final Random random,
137 final List<Double> result) {
138 result.clear();
139
140 // Calculate ECEF to NED coordinate transformation matrix
141 final var cen = CoordinateTransformation.ecefToNedMatrix(userLatitude, userLongitude);
142
143 // Loop satellites
144 for (final var satellitePosition : satellitePositions) {
145 result.add(generate(satellitePosition, userPosition, config, cen, random));
146 }
147 }
148
149 /**
150 * Generates bias.
151 *
152 * @param satellitePosition ECEF satellite position expressed in meters (m).
153 * @param userPosition ECEF user position expressed in meters (m).
154 * @param config GNSS configuration.
155 * @param cen ECEF to NED coordinate transformation matrix.
156 * @param random random number generator.
157 * @return generated bias provided satellite position.
158 */
159 @SuppressWarnings("DuplicatedCode")
160 private static double generate(final ECEFPosition satellitePosition, final ECEFPosition userPosition,
161 final GNSSConfig config, final Matrix cen, final Random random) {
162
163 // Determine ECEF line-of-sight vector using (8.41)
164 final var deltaRx = satellitePosition.getX() - userPosition.getX();
165 final var deltaRy = satellitePosition.getY() - userPosition.getY();
166 final var deltaRz = satellitePosition.getZ() - userPosition.getZ();
167
168 final var deltaRNorm = Math.sqrt(deltaRx * deltaRx + deltaRy * deltaRy + deltaRz * deltaRz);
169
170 final var uaseX = deltaRx / deltaRNorm;
171 final var uaseY = deltaRy / deltaRNorm;
172 final var uaseZ = deltaRz / deltaRNorm;
173
174 // Convert line of sight vector to NED using (8.39) and determine
175 // elevation using (8.57)
176 final var cen1 = cen.getElementAt(2, 0);
177 final var cen2 = cen.getElementAt(2, 1);
178 final var cen3 = cen.getElementAt(2, 2);
179
180 var elevation = -Math.asin(cen1 * uaseX + cen2 * uaseY + cen3 * uaseZ);
181
182 // Limit the minimum elevation angle to the masking angle
183 elevation = Math.max(elevation, Math.toRadians(config.getMaskAngleDegrees()));
184
185 // Calculate ionosphere and troposphere error SDs using (9.79) and (9.80)
186 final var cosElevation = Math.cos(elevation);
187 final var cosElevation2 = cosElevation * cosElevation;
188 final var ionoSD = config.getZenithIonosphereErrorSD() / Math.sqrt(1.0 - IONO_FACTOR * cosElevation2);
189 final var tropSD = config.getZenithTroposphereErrorSD() / Math.sqrt(1.0 - TROPO_FACTOR * cosElevation2);
190
191 // Determine range bias
192 return config.getSISErrorSD() * random.nextGaussian() + ionoSD * random.nextGaussian()
193 + tropSD * random.nextGaussian();
194 }
195 }