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.algebra.Utils;
20 import com.irurueta.algebra.WrongSizeException;
21 import com.irurueta.navigation.frames.CoordinateTransformation;
22 import com.irurueta.navigation.frames.ECEFPosition;
23 import com.irurueta.navigation.frames.ECEFVelocity;
24 import com.irurueta.navigation.frames.NEDPosition;
25 import com.irurueta.navigation.frames.NEDVelocity;
26 import com.irurueta.navigation.frames.converters.ECEFtoNEDPositionVelocityConverter;
27 import com.irurueta.navigation.geodesic.Constants;
28 import com.irurueta.units.Time;
29 import com.irurueta.units.TimeConverter;
30 import com.irurueta.units.TimeUnit;
31
32 import java.util.ArrayList;
33 import java.util.Collection;
34 import java.util.List;
35 import java.util.Random;
36
37 /**
38 * Generates satellite GNSS measurement data.
39 * This implementation is based on the equations defined in "Principles of GNSS, Inertial, and Multi-sensor
40 * Integrated Navigation Systems, Second Edition" and on the companion software available at:
41 * <a href="https://github.com/ymjdz/MATLAB-Codes/blob/master/Generate_GNSS_measurements.m">
42 * https://github.com/ymjdz/MATLAB-Codes/blob/master/Generate_GNSS_measurements.m
43 * </a>
44 */
45 @SuppressWarnings("DuplicatedCode")
46 public class GNSSMeasurementsGenerator {
47
48 /**
49 * Speed of light in the vacuum expressed in meters per second (m/s).
50 */
51 public static final double SPEED_OF_LIGHT = Constants.SPEED_OF_LIGHT;
52
53 /**
54 * Earth rotation rate expressed in radians per second (rad/s).
55 */
56 public static final double EARTH_ROTATION_RATE = Constants.EARTH_ROTATION_RATE;
57
58 /**
59 * Constructor.
60 * Prevents instantiation of utility class.
61 */
62 private GNSSMeasurementsGenerator() {
63 }
64
65 /**
66 * Generates satellite GNSS measurements.
67 *
68 * @param time current simulation time.
69 * @param satellitePositionsAndVelocities satellite positions and velocities.
70 * @param userPositionAndVelocity user position and velocity.
71 * @param gnssRangeErrorBiases GNSS range error biases for each
72 * satellite position and velocity.
73 * @param config GNSS configuration parameters.
74 * @param random random number generator.
75 * @return collection of GNSS measurements.
76 */
77 public static Collection<GNSSMeasurement> generate(
78 final Time time, final List<ECEFPositionAndVelocity> satellitePositionsAndVelocities,
79 final ECEFPositionAndVelocity userPositionAndVelocity, final List<Double> gnssRangeErrorBiases,
80 final GNSSConfig config, final Random random) {
81 return generate(convertTime(time), satellitePositionsAndVelocities, userPositionAndVelocity,
82 gnssRangeErrorBiases, config, random);
83 }
84
85 /**
86 * Generates satellite GNSS measurements.
87 *
88 * @param time current simulation time.
89 * @param satellitePositionsAndVelocities satellite positions and velocities.
90 * @param userPositionAndVelocity user position and velocity.
91 * @param gnssRangeErrorBiases GNSS range error biases for each
92 * satellite position and velocity.
93 * @param config GNSS configuration parameters.
94 * @param random random number generator.
95 * @param result instance where resulting collection of
96 * GNSS measurements are stored.
97 */
98 public static void generate(
99 final Time time, final List<ECEFPositionAndVelocity> satellitePositionsAndVelocities,
100 final ECEFPositionAndVelocity userPositionAndVelocity, final List<Double> gnssRangeErrorBiases,
101 final GNSSConfig config, final Random random, final Collection<GNSSMeasurement> result) {
102 generate(convertTime(time), satellitePositionsAndVelocities, userPositionAndVelocity, gnssRangeErrorBiases,
103 config, random, result);
104 }
105
106 /**
107 * Generates satellite GNSS measurements.
108 *
109 * @param time current simulation time expressed in
110 * seconds (s).
111 * @param satellitePositionsAndVelocities satellite positions and velocities.
112 * @param userPositionAndVelocity user position and velocity.
113 * @param gnssRangeErrorBiases GNSS range error biases for each
114 * satellite position and velocity.
115 * @param config GNSS configuration parameters.
116 * @param random random number generator.
117 * @return collection of GNSS measurements.
118 */
119 public static Collection<GNSSMeasurement> generate(
120 final double time, final List<ECEFPositionAndVelocity> satellitePositionsAndVelocities,
121 final ECEFPositionAndVelocity userPositionAndVelocity, final List<Double> gnssRangeErrorBiases,
122 final GNSSConfig config, final Random random) {
123 return generate(time, satellitePositionsAndVelocities, userPositionAndVelocity.getX(),
124 userPositionAndVelocity.getY(), userPositionAndVelocity.getZ(),
125 userPositionAndVelocity.getVx(), userPositionAndVelocity.getVy(),
126 userPositionAndVelocity.getVz(), gnssRangeErrorBiases, config, random);
127 }
128
129 /**
130 * Generates satellite GNSS measurements.
131 *
132 * @param time current simulation time expressed in
133 * seconds (s).
134 * @param satellitePositionsAndVelocities satellite positions and velocities.
135 * @param userPositionAndVelocity user position and velocity.
136 * @param gnssRangeErrorBiases GNSS range error biases for each
137 * satellite position and velocity.
138 * @param config GNSS configuration parameters.
139 * @param random random number generator.
140 * @param result instance where resulting collection of
141 * GNSS measurements are stored.
142 */
143 public static void generate(
144 final double time, final List<ECEFPositionAndVelocity> satellitePositionsAndVelocities,
145 final ECEFPositionAndVelocity userPositionAndVelocity, final List<Double> gnssRangeErrorBiases,
146 final GNSSConfig config, final Random random, final Collection<GNSSMeasurement> result) {
147 generate(time, satellitePositionsAndVelocities, userPositionAndVelocity.getX(),
148 userPositionAndVelocity.getY(), userPositionAndVelocity.getZ(),
149 userPositionAndVelocity.getVx(), userPositionAndVelocity.getVy(),
150 userPositionAndVelocity.getVz(), gnssRangeErrorBiases, config, random, result);
151 }
152
153 /**
154 * Generates satellite GNSS measurements.
155 *
156 * @param time current simulation time.
157 * @param satellitePositionsAndVelocities satellite positions and velocities.
158 * @param userPosition user position.
159 * @param userVelocity user velocity.
160 * @param gnssRangeErrorBiases GNSS range error biases for each satellite
161 * position and velocity.
162 * @param config GNSS configuration parameters.
163 * @param random random number generator.
164 * @return collection of GNSS measurements.
165 */
166 public static Collection<GNSSMeasurement> generate(
167 final Time time, final List<ECEFPositionAndVelocity> satellitePositionsAndVelocities,
168 final ECEFPosition userPosition, final ECEFVelocity userVelocity, final List<Double> gnssRangeErrorBiases,
169 final GNSSConfig config, final Random random) {
170 return generate(convertTime(time), satellitePositionsAndVelocities,
171 userPosition, userVelocity, gnssRangeErrorBiases, config, random);
172 }
173
174 /**
175 * Generates satellite GNSS measurements.
176 *
177 * @param time current simulation time.
178 * @param satellitePositionsAndVelocities satellite positions and velocities.
179 * @param userPosition user position.
180 * @param userVelocity user velocity.
181 * @param gnssRangeErrorBiases GNSS range error biases for each satellite
182 * position and velocity.
183 * @param config GNSS configuration parameters.
184 * @param random random number generator.
185 * @param result instance where resulting collection of
186 * GNSS measurements are stored.
187 */
188 public static void generate(
189 final Time time, final List<ECEFPositionAndVelocity> satellitePositionsAndVelocities,
190 final ECEFPosition userPosition, final ECEFVelocity userVelocity, final List<Double> gnssRangeErrorBiases,
191 final GNSSConfig config, final Random random, final Collection<GNSSMeasurement> result) {
192 generate(convertTime(time), satellitePositionsAndVelocities, userPosition, userVelocity, gnssRangeErrorBiases,
193 config, random, result);
194 }
195
196 /**
197 * Generates satellite GNSS measurements.
198 *
199 * @param time current simulation time expressed in
200 * seconds (s).
201 * @param satellitePositionsAndVelocities satellite positions and velocities.
202 * @param userPosition user position.
203 * @param userVelocity user velocity.
204 * @param gnssRangeErrorBiases GNSS range error biases for each satellite
205 * position and velocity.
206 * @param config GNSS configuration parameters.
207 * @param random random number generator.
208 * @return collection of GNSS measurements.
209 */
210 public static Collection<GNSSMeasurement> generate(
211 final double time, final List<ECEFPositionAndVelocity> satellitePositionsAndVelocities,
212 final ECEFPosition userPosition, final ECEFVelocity userVelocity, final List<Double> gnssRangeErrorBiases,
213 final GNSSConfig config, final Random random) {
214 return generate(time, satellitePositionsAndVelocities,
215 userPosition.getX(), userPosition.getY(), userPosition.getZ(),
216 userVelocity.getVx(), userVelocity.getVy(), userVelocity.getVz(), gnssRangeErrorBiases, config, random);
217 }
218
219 /**
220 * Generates satellite GNSS measurements.
221 *
222 * @param time current simulation time expressed in
223 * seconds (s).
224 * @param satellitePositionsAndVelocities satellite positions and velocities.
225 * @param userPosition user position.
226 * @param userVelocity user velocity.
227 * @param gnssRangeErrorBiases GNSS range error biases for each
228 * satellite position and velocity.
229 * @param config GNSS configuration parameters.
230 * @param random random number generator.
231 * @param result instance where resulting collection of
232 * GNSS measurements are stored.
233 */
234 public static void generate(
235 final double time, final List<ECEFPositionAndVelocity> satellitePositionsAndVelocities,
236 final ECEFPosition userPosition, final ECEFVelocity userVelocity, final List<Double> gnssRangeErrorBiases,
237 final GNSSConfig config, final Random random, final Collection<GNSSMeasurement> result) {
238 generate(time, satellitePositionsAndVelocities,
239 userPosition.getX(), userPosition.getY(), userPosition.getZ(),
240 userVelocity.getVx(), userVelocity.getVy(), userVelocity.getVz(), gnssRangeErrorBiases, config, random,
241 result);
242 }
243
244 /**
245 * Generates satellite GNSS measurements.
246 *
247 * @param time current simulation time expressed in
248 * seconds (s).
249 * @param satellitePositionsAndVelocities satellite positions and velocities.
250 * @param userX x ECEF coordinate of user position
251 * expressed in meters (m).
252 * @param userY y ECEF coordinate of user position
253 * expressed in meters (m).
254 * @param userZ z ECEF coordinate of user position
255 * expressed in meters (m).
256 * @param userVx x ECEF coordinate of user velocity
257 * expressed in meters per second (m/s).
258 * @param userVy y ECEF coordinate of user velocity
259 * expressed in meters per second (m/s).
260 * @param userVz z ECEF coordinate of user velocity
261 * expressed in meters per second (m/s).
262 * @param gnssRangeErrorBiases GNSS range error biases for each
263 * satellite position and velocity.
264 * @param config GNSS configuration parameters.
265 * @param random random number generator.
266 * @return collection of GNSS measurements.
267 */
268 public static Collection<GNSSMeasurement> generate(
269 final double time, final List<ECEFPositionAndVelocity> satellitePositionsAndVelocities,
270 final double userX, final double userY, final double userZ,
271 final double userVx, final double userVy, final double userVz,
272 final List<Double> gnssRangeErrorBiases, final GNSSConfig config, final Random random) {
273 final var result = new ArrayList<GNSSMeasurement>();
274 generate(time, satellitePositionsAndVelocities, userX, userY, userZ, userVx, userVy, userVz,
275 gnssRangeErrorBiases, config, random, result);
276 return result;
277 }
278
279 /**
280 * Generates satellite GNSS measurements.
281 *
282 * @param time current simulation time expressed in
283 * seconds (s).
284 * @param satellitePositionsAndVelocities satellite positions and velocities.
285 * @param userX x ECEF coordinate of user position
286 * expressed in meters (m).
287 * @param userY y ECEF coordinate of user position
288 * expressed in meters (m).
289 * @param userZ z ECEF coordinate of user position
290 * expressed in meters (m).
291 * @param userVx x ECEF coordinate of user velocity
292 * expressed in meters per second (m/s).
293 * @param userVy y ECEF coordinate of user velocity
294 * expressed in meters per second (m/s).
295 * @param userVz z ECEF coordinate of user velocity
296 * expressed in meters per second (m/s).
297 * @param gnssRangeErrorBiases GNSS range error biases for each
298 * satellite position and velocity.
299 * @param config GNSS configuration parameters.
300 * @param random random number generator.
301 * @param result instance where resulting collection of
302 * GNSS measurements are stored.
303 */
304 public static void generate(
305 final double time, final List<ECEFPositionAndVelocity> satellitePositionsAndVelocities,
306 final double userX, final double userY, final double userZ,
307 final double userVx, final double userVy, final double userVz,
308 final List<Double> gnssRangeErrorBiases, final GNSSConfig config,
309 final Random random, final Collection<GNSSMeasurement> result) {
310
311 if (satellitePositionsAndVelocities.size() != gnssRangeErrorBiases.size()) {
312 throw new IllegalArgumentException();
313 }
314
315 result.clear();
316
317 var pos = 0;
318 for (final var satellitePositionAndVelocity : satellitePositionsAndVelocities) {
319 final var gnssRangeErrorBias = gnssRangeErrorBiases.get(pos);
320 pos++;
321
322 if (gnssRangeErrorBias == null) {
323 continue;
324 }
325
326 final var measurement = generate(time,
327 satellitePositionAndVelocity.getX(),
328 satellitePositionAndVelocity.getY(),
329 satellitePositionAndVelocity.getZ(),
330 satellitePositionAndVelocity.getVx(),
331 satellitePositionAndVelocity.getVy(),
332 satellitePositionAndVelocity.getVz(),
333 userX, userY, userZ,
334 userVx, userVy, userVz,
335 gnssRangeErrorBias, config, random);
336 if (measurement != null) {
337 result.add(measurement);
338 }
339 }
340 }
341
342 /**
343 * Generates a single satellite GNSS measurement.
344 *
345 * @param time current simulation time.
346 * @param satellitePositionAndVelocity satellite position and velocity.
347 * @param userPositionAndVelocity user position and velocity.
348 * @param gnssRangeErrorBias GNSS range error bias.
349 * @param config GNSS configuration parameters.
350 * @param random random number generator.
351 * @return a new GNSS measurement.
352 */
353 public static GNSSMeasurement generate(
354 final Time time, final ECEFPositionAndVelocity satellitePositionAndVelocity,
355 final ECEFPositionAndVelocity userPositionAndVelocity, final double gnssRangeErrorBias,
356 final GNSSConfig config, final Random random) {
357 return generate(convertTime(time), satellitePositionAndVelocity, userPositionAndVelocity, gnssRangeErrorBias,
358 config, random);
359 }
360
361 /**
362 * Generates a single satellite GNSS measurement.
363 *
364 * @param time current simulation time.
365 * @param satellitePositionAndVelocity satellite position and velocity.
366 * @param userPositionAndVelocity user position and velocity.
367 * @param gnssRangeErrorBias GNSS range error bias.
368 * @param config GNSS configuration parameters.
369 * @param random random number generator.
370 * @param result instance where resulting GNSS measurement
371 * is stored.
372 * @return true if result has been obtained, failed if satellite is below elevation
373 * mask angle and result is not updated.
374 */
375 public static boolean generate(
376 final Time time, final ECEFPositionAndVelocity satellitePositionAndVelocity,
377 final ECEFPositionAndVelocity userPositionAndVelocity, final double gnssRangeErrorBias,
378 final GNSSConfig config, final Random random, final GNSSMeasurement result) {
379 return generate(convertTime(time), satellitePositionAndVelocity, userPositionAndVelocity, gnssRangeErrorBias,
380 config, random, result);
381 }
382
383 /**
384 * Generates a single satellite GNSS measurement.
385 *
386 * @param time current simulation time expressed in
387 * seconds (s).
388 * @param satellitePositionAndVelocity satellite position and velocity.
389 * @param userPositionAndVelocity user position and velocity.
390 * @param gnssRangeErrorBias GNSS range error bias.
391 * @param config GNSS configuration parameters.
392 * @param random random number generator.
393 * @return a new GNSS measurement.
394 */
395 public static GNSSMeasurement generate(
396 final double time, final ECEFPositionAndVelocity satellitePositionAndVelocity,
397 final ECEFPositionAndVelocity userPositionAndVelocity, final double gnssRangeErrorBias,
398 final GNSSConfig config, final Random random) {
399 return generate(time, satellitePositionAndVelocity.getX(),
400 satellitePositionAndVelocity.getY(), satellitePositionAndVelocity.getZ(),
401 satellitePositionAndVelocity.getVx(), satellitePositionAndVelocity.getVy(),
402 satellitePositionAndVelocity.getVz(), userPositionAndVelocity.getX(),
403 userPositionAndVelocity.getY(), userPositionAndVelocity.getZ(),
404 userPositionAndVelocity.getVx(), userPositionAndVelocity.getVy(),
405 userPositionAndVelocity.getVz(), gnssRangeErrorBias, config, random);
406 }
407
408 /**
409 * Generates a single satellite GNSS measurement.
410 *
411 * @param time current simulation time expressed in
412 * seconds (s).
413 * @param satellitePositionAndVelocity satellite position and velocity.
414 * @param userPositionAndVelocity user position and velocity.
415 * @param gnssRangeErrorBias GNSS range error bias.
416 * @param config GNSS configuration parameters.
417 * @param random random number generator.
418 * @param result instance where resulting GNSS measurement
419 * is stored.
420 * @return true if result has been obtained, failed if satellite is below elevation
421 * mask angle and result is not updated.
422 */
423 public static boolean generate(
424 final double time, final ECEFPositionAndVelocity satellitePositionAndVelocity,
425 final ECEFPositionAndVelocity userPositionAndVelocity, final double gnssRangeErrorBias,
426 final GNSSConfig config, final Random random, final GNSSMeasurement result) {
427 return generate(time, satellitePositionAndVelocity.getX(),
428 satellitePositionAndVelocity.getY(), satellitePositionAndVelocity.getZ(),
429 satellitePositionAndVelocity.getVx(), satellitePositionAndVelocity.getVy(),
430 satellitePositionAndVelocity.getVz(), userPositionAndVelocity.getX(),
431 userPositionAndVelocity.getY(), userPositionAndVelocity.getZ(),
432 userPositionAndVelocity.getVx(), userPositionAndVelocity.getVy(),
433 userPositionAndVelocity.getVz(), gnssRangeErrorBias, config, random, result);
434 }
435
436 /**
437 * Generates a single satellite GNSS measurement.
438 *
439 * @param time current simulation time.
440 * @param satellitePosition satellite position.
441 * @param satelliteVelocity satellite velocity.
442 * @param userPosition user position.
443 * @param userVelocity user velocity.
444 * @param gnssRangeErrorBias GNSS range error bias.
445 * @param config GNSS configuration parameters.
446 * @param random random number generator.
447 * @return a new GNSS measurement.
448 */
449 public static GNSSMeasurement generate(
450 final Time time, final ECEFPosition satellitePosition, final ECEFVelocity satelliteVelocity,
451 final ECEFPosition userPosition, final ECEFVelocity userVelocity, final double gnssRangeErrorBias,
452 final GNSSConfig config, final Random random) {
453 return generate(convertTime(time), satellitePosition, satelliteVelocity, userPosition, userVelocity,
454 gnssRangeErrorBias, config, random);
455 }
456
457 /**
458 * Generates a single satellite GNSS measurement.
459 *
460 * @param time current simulation time.
461 * @param satellitePosition satellite position.
462 * @param satelliteVelocity satellite velocity.
463 * @param userPosition user position.
464 * @param userVelocity user velocity.
465 * @param gnssRangeErrorBias GNSS range error bias.
466 * @param config GNSS configuration parameters.
467 * @param random random number generator.
468 * @param result instance where resulting GNSS measurement is stored.
469 * @return true if result has been obtained, failed if satellite is below elevation
470 * mask angle and result is not updated.
471 */
472 public static boolean generate(
473 final Time time, final ECEFPosition satellitePosition, final ECEFVelocity satelliteVelocity,
474 final ECEFPosition userPosition, final ECEFVelocity userVelocity, final double gnssRangeErrorBias,
475 final GNSSConfig config, final Random random, final GNSSMeasurement result) {
476 return generate(convertTime(time), satellitePosition, satelliteVelocity, userPosition, userVelocity,
477 gnssRangeErrorBias, config, random, result);
478 }
479
480 /**
481 * Generates a single satellite GNSS measurement.
482 *
483 * @param time current simulation time expressed in seconds (s).
484 * @param satellitePosition satellite position.
485 * @param satelliteVelocity satellite velocity.
486 * @param userPosition user position.
487 * @param userVelocity user velocity.
488 * @param gnssRangeErrorBias GNSS range error bias.
489 * @param config GNSS configuration parameters.
490 * @param random random number generator.
491 * @return a new GNSS measurement.
492 */
493 public static GNSSMeasurement generate(
494 final double time, final ECEFPosition satellitePosition, final ECEFVelocity satelliteVelocity,
495 final ECEFPosition userPosition, final ECEFVelocity userVelocity, final double gnssRangeErrorBias,
496 final GNSSConfig config, final Random random) {
497 return generate(time, satellitePosition.getX(), satellitePosition.getY(), satellitePosition.getZ(),
498 satelliteVelocity.getVx(), satelliteVelocity.getVy(), satelliteVelocity.getVz(),
499 userPosition.getX(), userPosition.getY(), userPosition.getZ(),
500 userVelocity.getVx(), userVelocity.getVy(), userVelocity.getVz(), gnssRangeErrorBias, config, random);
501 }
502
503 /**
504 * Generates a single satellite GNSS measurement.
505 *
506 * @param time current simulation time expressed in seconds (s).
507 * @param satellitePosition satellite position.
508 * @param satelliteVelocity satellite velocity.
509 * @param userPosition user position.
510 * @param userVelocity user velocity.
511 * @param gnssRangeErrorBias GNSS range error bias.
512 * @param config GNSS configuration parameters.
513 * @param random random number generator.
514 * @param result instance where resulting GNSS measurement
515 * is stored.
516 * @return true if result has been obtained, failed if satellite is below elevation
517 * mask angle and result is not updated.
518 */
519 public static boolean generate(
520 final double time, final ECEFPosition satellitePosition, final ECEFVelocity satelliteVelocity,
521 final ECEFPosition userPosition, final ECEFVelocity userVelocity, final double gnssRangeErrorBias,
522 final GNSSConfig config, final Random random, final GNSSMeasurement result) {
523 return generate(time, satellitePosition.getX(), satellitePosition.getY(), satellitePosition.getZ(),
524 satelliteVelocity.getVx(), satelliteVelocity.getVy(), satelliteVelocity.getVz(),
525 userPosition.getX(), userPosition.getY(), userPosition.getZ(),
526 userVelocity.getVx(), userVelocity.getVy(), userVelocity.getVz(),
527 gnssRangeErrorBias, config, random, result);
528 }
529
530 /**
531 * Generates a single satellite GNSS measurement.
532 *
533 * @param time current simulation time expressed in seconds (s).
534 * @param satelliteX x ECEF coordinate of satellite position
535 * expressed in meters (m).
536 * @param satelliteY y ECEF coordinate of satellite position
537 * expressed in meters (m).
538 * @param satelliteZ z ECEF coordinate of satellite position
539 * expressed in meters (m).
540 * @param satelliteVx x ECEF coordinate of satellite velocity
541 * expressed in meters per second (m/s).
542 * @param satelliteVy y ECEF coordinate of satellite velocity
543 * expressed in meters per second (m/s).
544 * @param satelliteVz z ECEF coordinate of satellite velocity
545 * expressed in meters per second (m/s).
546 * @param userX x ECEF coordinate of user position
547 * expressed in meters (m).
548 * @param userY y ECEF coordinate of user position
549 * expressed in meters (m).
550 * @param userZ z ECEF coordinate of user position
551 * expressed in meters (m).
552 * @param userVx x ECEF coordinate of user velocity
553 * expressed in meters per second (m/s).
554 * @param userVy y ECEF coordinate of user velocity
555 * expressed in meters per second (m/s).
556 * @param userVz z ECEF coordinate of user velocity
557 * expressed in meters per second (m/s).
558 * @param gnssRangeErrorBias GNSS range error bias.
559 * @param config GNSS configuration parameters.
560 * @param random random number generator.
561 * @return a new GNSS measurement.
562 */
563 public static GNSSMeasurement generate(
564 final double time, final double satelliteX, final double satelliteY, final double satelliteZ,
565 final double satelliteVx, final double satelliteVy, final double satelliteVz,
566 final double userX, final double userY, final double userZ,
567 final double userVx, final double userVy, final double userVz,
568 final double gnssRangeErrorBias, final GNSSConfig config, final Random random) {
569 final var result = new GNSSMeasurement();
570 if (generate(time, satelliteX, satelliteY, satelliteZ, satelliteVx, satelliteVy, satelliteVz,
571 userX, userY, userZ, userVx, userVy, userVz, gnssRangeErrorBias, config, random, result)) {
572 return result;
573 } else {
574 return null;
575 }
576 }
577
578 /**
579 * Generates a single satellite GNSS measurement.
580 *
581 * @param time current simulation time expressed in seconds (s).
582 * @param satelliteX x ECEF coordinate of satellite position
583 * expressed in meters (m).
584 * @param satelliteY y ECEF coordinate of satellite position
585 * expressed in meters (m).
586 * @param satelliteZ z ECEF coordinate of satellite position
587 * expressed in meters (m).
588 * @param satelliteVx x ECEF coordinate of satellite velocity
589 * expressed in meters per second (m/s).
590 * @param satelliteVy y ECEF coordinate of satellite velocity
591 * expressed in meters per second (m/s).
592 * @param satelliteVz z ECEF coordinate of satellite velocity
593 * expressed in meters per second (m/s).
594 * @param userX x ECEF coordinate of user position
595 * expressed in meters (m).
596 * @param userY y ECEF coordinate of user position
597 * expressed in meters (m).
598 * @param userZ z ECEF coordinate of user position
599 * expressed in meters (m).
600 * @param userVx x ECEF coordinate of user velocity
601 * expressed in meters per second (m/s).
602 * @param userVy y ECEF coordinate of user velocity
603 * expressed in meters per second (m/s).
604 * @param userVz z ECEF coordinate of user velocity
605 * expressed in meters per second (m/s).
606 * @param gnssRangeErrorBias GNSS range error bias.
607 * @param config GNSS configuration parameters.
608 * @param random random number generator.
609 * @param result instance where resulting GNSS measurement
610 * is stored.
611 * @return true if result has been obtained, failed if satellite is below elevation
612 * mask angle and result is not updated.
613 */
614 public static boolean generate(
615 final double time, final double satelliteX, final double satelliteY, final double satelliteZ,
616 final double satelliteVx, final double satelliteVy, final double satelliteVz,
617 final double userX, final double userY, final double userZ,
618 final double userVx, final double userVy, final double userVz, final double gnssRangeErrorBias,
619 final GNSSConfig config, final Random random, final GNSSMeasurement result) {
620 final var userNedPosition = new NEDPosition();
621 final var userNedVelocity = new NEDVelocity();
622 ECEFtoNEDPositionVelocityConverter.convertECEFtoNED(userX, userY, userZ, userVx, userVy, userVz,
623 userNedPosition, userNedVelocity);
624 final var userLatitude = userNedPosition.getLatitude();
625 final var userLongitude = userNedPosition.getLongitude();
626
627 return generate(time, satelliteX, satelliteY, satelliteZ, satelliteVx, satelliteVy, satelliteVz,
628 userX, userY, userZ, userLatitude, userLongitude, userVx, userVy, userVz, gnssRangeErrorBias,
629 config, random, result);
630 }
631
632 /**
633 * Internal method to generate a single satellite GNSS measurement.
634 *
635 * @param time current simulation time expressed in seconds (s).
636 * @param satelliteX x ECEF coordinate of satellite position
637 * expressed in meters (m).
638 * @param satelliteY y ECEF coordinate of satellite position
639 * expressed in meters (m).
640 * @param satelliteZ z ECEF coordinate of satellite position
641 * expressed in meters (m).
642 * @param satelliteVx x ECEF coordinate of satellite velocity
643 * expressed in meters per second (m/s).
644 * @param satelliteVy y ECEF coordinate of satellite velocity
645 * expressed in meters per second (m/s).
646 * @param satelliteVz z ECEF coordinate of satellite velocity
647 * expressed in meters per second (m/s).
648 * @param userX x ECEF coordinate of user position
649 * expressed in meters (m).
650 * @param userY y ECEF coordinate of user position
651 * expressed in meters (m).
652 * @param userZ z ECEF coordinate of user position
653 * expressed in meters (m).
654 * @param userLatitude latitude of user NED position expressed in
655 * radians (rad).
656 * @param userLongitude longitude of user NED position expressed in
657 * radians (rad).
658 * @param userVx x ECEF coordinate of user velocity
659 * expressed in meters per second (m/s).
660 * @param userVy y ECEF coordinate of user velocity
661 * expressed in meters per second (m/s).
662 * @param userVz z ECEF coordinate of user velocity
663 * expressed in meters per second (m/s).
664 * @param gnssRangeErrorBias GNSS range error bias.
665 * @param config GNSS configuration parameters.
666 * @param random random number generator.
667 * @param result instance where resulting GNSS measurement
668 * is stored.
669 * @return true if result has been obtained, failed if satellite is below elevation
670 * mask angle and result is not updated.
671 */
672 private static boolean generate(
673 final double time, final double satelliteX, final double satelliteY, final double satelliteZ,
674 final double satelliteVx, final double satelliteVy, final double satelliteVz,
675 final double userX, final double userY, final double userZ,
676 final double userLatitude, final double userLongitude,
677 final double userVx, final double userVy, final double userVz,
678 final double gnssRangeErrorBias, final GNSSConfig config, final Random random,
679 final GNSSMeasurement result) {
680
681 // Calculate ECEF to NED coordinate transformation matrix using (2.150)
682 final var cen = CoordinateTransformation.ecefToNedMatrix(userLatitude, userLongitude);
683
684 // Skew symmetric matrix of Earth rate
685 try {
686 final var omegaIe = Utils.skewMatrix(new double[]{0.0, 0.0, EARTH_ROTATION_RATE});
687 final var cei = Matrix.identity(CoordinateTransformation.ROWS, CoordinateTransformation.COLS);
688 final var satellitePosition = new Matrix(CoordinateTransformation.ROWS, 1);
689 final var deltaR = new Matrix(CoordinateTransformation.ROWS, 1);
690 final var satelliteVelocity = new Matrix(CoordinateTransformation.ROWS, 1);
691 final var userPosition = new Matrix(CoordinateTransformation.ROWS, 1);
692 final var userVelocity = new Matrix(CoordinateTransformation.ROWS, 1);
693 final var tmp1 = new Matrix(CoordinateTransformation.ROWS, 1);
694 final var tmp2 = new Matrix(CoordinateTransformation.ROWS, 1);
695 final var tmp3 = new Matrix(CoordinateTransformation.ROWS, 1);
696 final var tmp4 = new Matrix(CoordinateTransformation.ROWS, 1);
697 final var tmp5 = new Matrix(CoordinateTransformation.ROWS, 1);
698 final var tmp6 = new Matrix(CoordinateTransformation.ROWS, 1);
699
700 return generate(time, satelliteX, satelliteY, satelliteZ, satelliteVx, satelliteVy, satelliteVz,
701 userX, userY, userZ, userVx, userVy, userVz, gnssRangeErrorBias, config, cen, omegaIe, cei,
702 satellitePosition, deltaR, satelliteVelocity, userPosition, userVelocity, tmp1, tmp2, tmp3, tmp4,
703 tmp5, tmp6, random, result);
704 } catch (final WrongSizeException ignore) {
705 return false;
706 }
707 }
708
709 /**
710 * Internal method to generate a single satellite GNSS measurement.
711 *
712 * @param time current simulation time expressed in seconds (s).
713 * @param satelliteX x ECEF coordinate of satellite position
714 * expressed in meters (m).
715 * @param satelliteY y ECEF coordinate of satellite position
716 * expressed in meters (m).
717 * @param satelliteZ z ECEF coordinate of satellite position
718 * expressed in meters (m).
719 * @param satelliteVx x ECEF coordinate of satellite velocity
720 * expressed in meters per second (m/s).
721 * @param satelliteVy y ECEF coordinate of satellite velocity
722 * expressed in meters per second (m/s).
723 * @param satelliteVz z ECEF coordinate of satellite velocity
724 * expressed in meters per second (m/s).
725 * @param userX x ECEF coordinate of user position
726 * expressed in meters (m).
727 * @param userY y ECEF coordinate of user position
728 * expressed in meters (m).
729 * @param userZ z ECEF coordinate of user position
730 * expressed in meters (m).
731 * @param userVx x ECEF coordinate of user velocity
732 * expressed in meters per second (m/s).
733 * @param userVy y ECEF coordinate of user velocity
734 * expressed in meters per second (m/s).
735 * @param userVz z ECEF coordinate of user velocity
736 * expressed in meters per second (m/s).
737 * @param gnssRangeErrorBias GNSS range error bias.
738 * @param config GNSS configuration parameters.
739 * @param cen ECEF to NED coordinate transformation matrix.
740 * @param omegaIe skew symmetric matrix of Earth rotation rate.
741 * @param cei ECEF to ECI conversion matrix.
742 * @param satellitePosition column matrix containing satellite position.
743 * @param deltaR vector containing satellite to user position
744 * difference.
745 * @param satelliteVelocity column matrix containing satellite velocity.
746 * @param userPosition column matrix containing user position.
747 * @param userVelocity column matrix containing user velocity.
748 * @param tmp1 column matrix containing temporal values.
749 * @param tmp2 column matrix containing temporal values.
750 * @param tmp3 column matrix containing temporal values.
751 * @param tmp4 column matrix containing temporal values.
752 * @param tmp5 column matrix containing temporal values.
753 * @param tmp6 column matrix containing temporal values.
754 * @param random random number generator.
755 * @param result instance where resulting GNSS measurement
756 * is stored.
757 * @return true if result has been obtained, failed if satellite is below elevation
758 * mask angle and result is not updated.
759 * @throws WrongSizeException if an error occurs.
760 */
761 private static boolean generate(
762 final double time, final double satelliteX, final double satelliteY, final double satelliteZ,
763 final double satelliteVx, final double satelliteVy, final double satelliteVz,
764 final double userX, final double userY, final double userZ,
765 final double userVx, final double userVy, final double userVz,
766 final double gnssRangeErrorBias, final GNSSConfig config,
767 final Matrix cen, final Matrix omegaIe, final Matrix cei,
768 final Matrix satellitePosition, final Matrix deltaR,
769 final Matrix satelliteVelocity, final Matrix userPosition,
770 final Matrix userVelocity, final Matrix tmp1, final Matrix tmp2,
771 final Matrix tmp3, final Matrix tmp4, final Matrix tmp5, final Matrix tmp6,
772 final Random random, final GNSSMeasurement result) throws WrongSizeException {
773
774 // Determine ECEF line-of-sight vector using (8.41)
775 final var deltaRx = satelliteX - userX;
776 final var deltaRy = satelliteY - userY;
777 final var deltaRz = satelliteZ - userZ;
778
779 final var approxRange = Math.sqrt(deltaRx * deltaRx + deltaRy * deltaRy + deltaRz * deltaRz);
780
781 final var uaseX = deltaRx / approxRange;
782 final var uaseY = deltaRy / approxRange;
783 final var uaseZ = deltaRz / approxRange;
784
785 // Convert line-of-sight vector to NED using (8.39) and determine
786 // elevation using (8.57)
787 final var cen1 = cen.getElementAt(2, 0);
788 final var cen2 = cen.getElementAt(2, 1);
789 final var cen3 = cen.getElementAt(2, 2);
790
791 final var elevation = -Math.asin(cen1 * uaseX + cen2 * uaseY + cen3 * uaseZ);
792
793 // Determine if satellite is above the masking angle
794 if (elevation >= Math.toRadians(config.getMaskAngleDegrees())) {
795
796 // Calculate frame rotation during signal transit time using (8.36)
797 final var ceiValue = EARTH_ROTATION_RATE * approxRange / SPEED_OF_LIGHT;
798 cei.setElementAt(0, 1, ceiValue);
799 cei.setElementAt(1, 0, -ceiValue);
800
801 // Calculate range using (8.35)
802 satellitePosition.setElementAtIndex(0, satelliteX);
803 satellitePosition.setElementAtIndex(1, satelliteY);
804 satellitePosition.setElementAtIndex(2, satelliteZ);
805
806 cei.multiply(satellitePosition, deltaR);
807
808 deltaR.setElementAtIndex(0, deltaR.getElementAtIndex(0) - userX);
809 deltaR.setElementAtIndex(1, deltaR.getElementAtIndex(1) - userY);
810 deltaR.setElementAtIndex(2, deltaR.getElementAtIndex(2) - userZ);
811
812 final var range = Utils.normF(deltaR);
813
814 // Calculate range rate using (8.44)
815
816 satelliteVelocity.setElementAtIndex(0, satelliteVx);
817 satelliteVelocity.setElementAtIndex(1, satelliteVy);
818 satelliteVelocity.setElementAtIndex(2, satelliteVz);
819
820 omegaIe.multiply(satellitePosition, tmp1);
821
822 satelliteVelocity.add(tmp1, tmp2);
823
824 cei.multiply(tmp2, tmp3);
825
826 userPosition.setElementAtIndex(0, userX);
827 userPosition.setElementAtIndex(1, userY);
828 userPosition.setElementAtIndex(2, userZ);
829
830 omegaIe.multiply(userPosition, tmp4);
831
832 userVelocity.setElementAtIndex(0, userVx);
833 userVelocity.setElementAtIndex(1, userVy);
834 userVelocity.setElementAtIndex(2, userVz);
835
836 userVelocity.add(tmp4, tmp5);
837
838 tmp3.subtract(tmp5, tmp6);
839
840 final var rangeRate = uaseX * tmp6.getElementAtIndex(0) + uaseY * tmp6.getElementAtIndex(1)
841 + uaseZ * tmp6.getElementAtIndex(2);
842
843 // Calculate pseudo-range measurement
844 final var pseudoRange = range + gnssRangeErrorBias + config.getInitialReceiverClockOffset()
845 + config.getInitialReceiverClockDrift() * time
846 + config.getCodeTrackingErrorSD() * random.nextGaussian();
847
848 // Calculate pseudo-range rate measurement
849 final var pseudoRate = rangeRate + config.getInitialReceiverClockDrift()
850 + config.getRangeRateTrackingErrorSD() * random.nextGaussian();
851
852 // Set result values
853 result.setPseudoRange(pseudoRange);
854 result.setPseudoRate(pseudoRate);
855 result.setPositionCoordinates(satelliteX, satelliteY, satelliteZ);
856 result.setVelocityCoordinates(satelliteVx, satelliteVy, satelliteVz);
857
858 // Indicate that result has been updated
859 return true;
860 } else {
861 // Indicate that result is not updated
862 return false;
863 }
864 }
865
866 /**
867 * Converts time instance into seconds.
868 *
869 * @param time instance to be converted.
870 * @return time converted to seconds.
871 */
872 private static double convertTime(final Time time) {
873 return TimeConverter.convert(time.getValue().doubleValue(), time.getUnit(), TimeUnit.SECOND);
874 }
875 }