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.inertial.estimators;
17
18 import com.irurueta.navigation.frames.NEDPosition;
19 import com.irurueta.navigation.frames.NEDVelocity;
20 import com.irurueta.units.*;
21
22 /**
23 * Estimates curvilinear position by integrating the velocity.
24 * This implementation is based on the equations defined in "Principles of GNSS, Inertial, and Multisensor
25 * Integrated Navigation Systems, Second Edition" and on the companion software available at:
26 * <a href="https://github.com/ymjdz/MATLAB-Codes/blob/master/Update_curvilinear_position.m">
27 * https://github.com/ymjdz/MATLAB-Codes/blob/master/Update_curvilinear_position.m
28 * </a>
29 */
30 public class NEDPositionEstimator {
31
32 /**
33 * Estimates curvilinear position by integrating the velocity.
34 *
35 * @param timeInterval time interval between epochs expressed in seconds (s).
36 * @param oldLatitude previous latitude expressed in radians (rad).
37 * @param oldLongitude previous longitude expressed in radians (rad).
38 * @param oldHeight previous height expressed in meters (m).
39 * @param oldVn previous velocity of body with respect the Earth, resolved about
40 * north-axis and expressed in meters per second (m/s).
41 * @param oldVe previous velocity of body with respect the Earth, resolved about
42 * east-axis and expressed in meters per second (m/s).
43 * @param oldVd previous velocity of body with respect the Earth, resolved about
44 * down-axis and expressed in meters per second (m/s).
45 * @param vn current velocity of body with respect the Earth, resolved about
46 * north-axis and expressed in meters per second (m/s).
47 * @param ve current velocity of body with respect the Earth, resolved about
48 * east-axis and expressed in meters per second (m/s).
49 * @param vd current velocity of body with respect the Earth, resolved about
50 * down-axis and expressed in meters per second (m/s).
51 * @param result instance where updated curvilinear position will be stored.
52 * @throws IllegalArgumentException if provided time interval is negative.
53 */
54 public void estimate(
55 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
56 final double oldVn, final double oldVe, final double oldVd,
57 final double vn, final double ve, final double vd, final NEDPosition result) {
58 estimatePosition(timeInterval, oldLatitude, oldLongitude, oldHeight, oldVn, oldVe, oldVd, vn, ve, vd, result);
59 }
60
61 /**
62 * Estimates curvilinear position by integrating the velocity.
63 *
64 * @param timeInterval time interval between epochs.
65 * @param oldLatitude previous latitude expressed in radians (rad).
66 * @param oldLongitude previous longitude expressed in radians (rad).
67 * @param oldHeight previous height expressed in meters (m).
68 * @param oldVn previous velocity of body with respect the Earth, resolved about
69 * north-axis and expressed in meters per second (m/s).
70 * @param oldVe previous velocity of body with respect the Earth, resolved about
71 * east-axis and expressed in meters per second (m/s).
72 * @param oldVd previous velocity of body with respect the Earth, resolved about
73 * down-axis and expressed in meters per second (m/s).
74 * @param vn current velocity of body with respect the Earth, resolved about
75 * north-axis and expressed in meters per second (m/s).
76 * @param ve current velocity of body with respect the Earth, resolved about
77 * east-axis and expressed in meters per second (m/s).
78 * @param vd current velocity of body with respect the Earth, resolved about
79 * down-axis and expressed in meters per second (m/s).
80 * @param result instance where updated curvilinear position will be stored.
81 * @throws IllegalArgumentException if provided time interval is negative.
82 */
83 public void estimate(
84 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
85 final double oldVn, final double oldVe, final double oldVd,
86 final double vn, final double ve, final double vd, final NEDPosition result) {
87 estimatePosition(timeInterval, oldLatitude, oldLongitude, oldHeight, oldVn, oldVe, oldVd, vn, ve, vd, result);
88 }
89
90 /**
91 * Estimates curvilinear position by integrating the velocity.
92 *
93 * @param timeInterval time interval between epochs expressed in seconds (s).
94 * @param oldLatitude previous latitude.
95 * @param oldLongitude previous longitude.
96 * @param oldHeight previous height.
97 * @param oldVn previous velocity of body with respect the Earth, resolved about
98 * north-axis.
99 * @param oldVe previous velocity of body with respect the Earth, resolved about
100 * east-axis.
101 * @param oldVd previous velocity of body with respect the Earth, resolved about
102 * down-axis.
103 * @param vn current velocity of body with respect the Earth, resolved about
104 * north-axis.
105 * @param ve current velocity of body with respect the Earth, resolved about
106 * east-axis.
107 * @param vd current velocity of body with respect the Earth, resolved about
108 * down-axis.
109 * @param result instance where updated curvilinear position will be stored.
110 * @throws IllegalArgumentException if provided time interval is negative.
111 */
112 public void estimate(
113 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
114 final Speed oldVn, final Speed oldVe, final Speed oldVd, final Speed vn, final Speed ve, final Speed vd,
115 final NEDPosition result) {
116 estimatePosition(timeInterval, oldLatitude, oldLongitude, oldHeight, oldVn, oldVe, oldVd, vn, ve, vd, result);
117 }
118
119 /**
120 * Estimates curvilinear position by integrating the velocity.
121 *
122 * @param timeInterval time interval between epochs.
123 * @param oldLatitude previous latitude.
124 * @param oldLongitude previous longitude.
125 * @param oldHeight previous height.
126 * @param oldVn previous velocity of body with respect the Earth, resolved about
127 * north-axis.
128 * @param oldVe previous velocity of body with respect the Earth, resolved about
129 * east-axis.
130 * @param oldVd previous velocity of body with respect the Earth, resolved about
131 * down-axis.
132 * @param vn current velocity of body with respect the Earth, resolved about
133 * north-axis.
134 * @param ve current velocity of body with respect the Earth, resolved about
135 * east-axis.
136 * @param vd current velocity of body with respect the Earth, resolved about
137 * down-axis.
138 * @param result instance where updated curvilinear position will be stored.
139 * @throws IllegalArgumentException if provided time interval is negative.
140 */
141 public void estimate(
142 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
143 final Speed oldVn, final Speed oldVe, final Speed oldVd, final Speed vn, final Speed ve, final Speed vd,
144 final NEDPosition result) {
145 estimatePosition(timeInterval, oldLatitude, oldLongitude, oldHeight, oldVn, oldVe, oldVd, vn, ve, vd, result);
146 }
147
148 /**
149 * Estimates curvilinear position by integrating the velocity.
150 *
151 * @param timeInterval time interval between epochs expressed in seconds (s).
152 * @param oldLatitude previous latitude.
153 * @param oldLongitude previous longitude.
154 * @param oldHeight previous height.
155 * @param oldVn previous velocity of body with respect the Earth, resolved about
156 * north-axis and expressed in meters per second (m/s).
157 * @param oldVe previous velocity of body with respect the Earth, resolved about
158 * east-axis and expressed in meters per second (m/s).
159 * @param oldVd previous velocity of body with respect the Earth, resolved about
160 * down-axis and expressed in meters per second (m/s).
161 * @param vn current velocity of body with respect the Earth, resolved about
162 * north-axis and expressed in meters per second (m/s).
163 * @param ve current velocity of body with respect the Earth, resolved about
164 * east-axis and expressed in meters per second (m/s).
165 * @param vd current velocity of body with respect the Earth, resolved about
166 * down-axis and expressed in meters per second (m/s).
167 * @param result instance where updated curvilinear position will be stored.
168 * @throws IllegalArgumentException if provided time interval is negative.
169 */
170 public void estimate(
171 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
172 final double oldVn, final double oldVe, final double oldVd,
173 final double vn, final double ve, final double vd, final NEDPosition result) {
174 estimatePosition(timeInterval, oldLatitude, oldLongitude, oldHeight, oldVn, oldVe, oldVd, vn, ve, vd, result);
175 }
176
177 /**
178 * Estimates curvilinear position by integrating the velocity.
179 *
180 * @param timeInterval time interval between epochs.
181 * @param oldLatitude previous latitude.
182 * @param oldLongitude previous longitude.
183 * @param oldHeight previous height.
184 * @param oldVn previous velocity of body with respect the Earth, resolved about
185 * north-axis and expressed in meters per second (m/s).
186 * @param oldVe previous velocity of body with respect the Earth, resolved about
187 * east-axis and expressed in meters per second (m/s).
188 * @param oldVd previous velocity of body with respect the Earth, resolved about
189 * down-axis and expressed in meters per second (m/s).
190 * @param vn current velocity of body with respect the Earth, resolved about
191 * north-axis and expressed in meters per second (m/s).
192 * @param ve current velocity of body with respect the Earth, resolved about
193 * east-axis and expressed in meters per second (m/s).
194 * @param vd current velocity of body with respect the Earth, resolved about
195 * down-axis and expressed in meters per second (m/s).
196 * @param result instance where updated curvilinear position will be stored.
197 * @throws IllegalArgumentException if provided time interval is negative.
198 */
199 public void estimate(
200 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
201 final double oldVn, final double oldVe, final double oldVd,
202 final double vn, final double ve, final double vd, final NEDPosition result) {
203 estimatePosition(timeInterval, oldLatitude, oldLongitude, oldHeight, oldVn, oldVe, oldVd, vn, ve, vd, result);
204 }
205
206 /**
207 * Estimates curvilinear position by integrating the velocity.
208 *
209 * @param timeInterval time interval between epochs expressed in seconds (s).
210 * @param oldLatitude previous latitude expressed in radians (rad).
211 * @param oldLongitude previous longitude expressed in radians (rad).
212 * @param oldHeight previous height expressed in meters (m).
213 * @param oldVn previous velocity of body with respect the Earth, resolved about
214 * north-axis.
215 * @param oldVe previous velocity of body with respect the Earth, resolved about
216 * east-axis.
217 * @param oldVd previous velocity of body with respect the Earth, resolved about
218 * down-axis.
219 * @param vn current velocity of body with respect the Earth, resolved about
220 * north-axis.
221 * @param ve current velocity of body with respect the Earth, resolved about
222 * east-axis.
223 * @param vd current velocity of body with respect the Earth, resolved about
224 * down-axis.
225 * @param result instance where updated curvilinear position will be stored.
226 * @throws IllegalArgumentException if provided time interval is negative.
227 */
228 public void estimate(
229 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
230 final Speed oldVn, final Speed oldVe, final Speed oldVd, final Speed vn, final Speed ve, final Speed vd,
231 final NEDPosition result) {
232 estimatePosition(timeInterval, oldLatitude, oldLongitude, oldHeight, oldVn, oldVe, oldVd, vn, ve, vd, result);
233 }
234
235 /**
236 * Estimates curvilinear position by integrating the velocity.
237 *
238 * @param timeInterval time interval between epochs.
239 * @param oldLatitude previous latitude expressed in radians (rad).
240 * @param oldLongitude previous longitude expressed in radians (rad).
241 * @param oldHeight previous height expressed in meters (m).
242 * @param oldVn previous velocity of body with respect the Earth, resolved about
243 * north-axis.
244 * @param oldVe previous velocity of body with respect the Earth, resolved about
245 * east-axis.
246 * @param oldVd previous velocity of body with respect the Earth, resolved about
247 * down-axis.
248 * @param vn current velocity of body with respect the Earth, resolved about
249 * north-axis.
250 * @param ve current velocity of body with respect the Earth, resolved about
251 * east-axis.
252 * @param vd current velocity of body with respect the Earth, resolved about
253 * down-axis.
254 * @param result instance where updated curvilinear position will be stored.
255 * @throws IllegalArgumentException if provided time interval is negative.
256 */
257 public void estimate(
258 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
259 final Speed oldVn, final Speed oldVe, final Speed oldVd, final Speed vn, final Speed ve, final Speed vd,
260 final NEDPosition result) {
261 estimatePosition(timeInterval, oldLatitude, oldLongitude, oldHeight, oldVn, oldVe, oldVd, vn, ve, vd, result);
262 }
263
264 /**
265 * Estimates curvilinear position by integrating the velocity.
266 *
267 * @param timeInterval time interval between epochs expressed in seconds (s).
268 * @param oldPosition previous body position with respect the Earth, resolved about
269 * north, east and down axes.
270 * @param oldVn previous velocity of body with respect the Earth, resolved about
271 * north-axis and expressed in meters per second (m/s).
272 * @param oldVe previous velocity of body with respect the Earth, resolved about
273 * east-axis and expressed in meters per second (m/s).
274 * @param oldVd previous velocity of body with respect the Earth, resolved about
275 * down-axis and expressed in meters per second (m/s).
276 * @param vn current velocity of body with respect the Earth, resolved about
277 * north-axis and expressed in meters per second (m/s).
278 * @param ve current velocity of body with respect the Earth, resolved about
279 * east-axis and expressed in meters per second (m/s).
280 * @param vd current velocity of body with respect the Earth, resolved about
281 * down-axis and expressed in meters per second (m/s).
282 * @param result instance where updated curvilinear position will be stored.
283 * @throws IllegalArgumentException if provided time interval is negative.
284 */
285 public void estimate(
286 final double timeInterval, final NEDPosition oldPosition,
287 final double oldVn, final double oldVe, final double oldVd,
288 final double vn, final double ve, final double vd, final NEDPosition result) {
289 estimatePosition(timeInterval, oldPosition, oldVn, oldVe, oldVd, vn, ve, vd, result);
290 }
291
292 /**
293 * Estimates curvilinear position by integrating the velocity.
294 *
295 * @param timeInterval time interval between epochs.
296 * @param oldPosition previous body position with respect the Earth, resolved about
297 * north, east and down axes.
298 * @param oldVn previous velocity of body with respect the Earth, resolved about
299 * north-axis and expressed in meters per second (m/s).
300 * @param oldVe previous velocity of body with respect the Earth, resolved about
301 * east-axis and expressed in meters per second (m/s).
302 * @param oldVd previous velocity of body with respect the Earth, resolved about
303 * down-axis and expressed in meters per second (m/s).
304 * @param vn current velocity of body with respect the Earth, resolved about
305 * north-axis and expressed in meters per second (m/s).
306 * @param ve current velocity of body with respect the Earth, resolved about
307 * east-axis and expressed in meters per second (m/s).
308 * @param vd current velocity of body with respect the Earth, resolved about
309 * down-axis and expressed in meters per second (m/s).
310 * @param result instance where updated curvilinear position will be stored.
311 * @throws IllegalArgumentException if provided time interval is negative.
312 */
313 public void estimate(
314 final Time timeInterval, final NEDPosition oldPosition,
315 final double oldVn, final double oldVe, final double oldVd,
316 final double vn, final double ve, final double vd, final NEDPosition result) {
317 estimatePosition(timeInterval, oldPosition, oldVn, oldVe, oldVd, vn, ve, vd, result);
318 }
319
320 /**
321 * Estimates curvilinear position by integrating the velocity.
322 *
323 * @param timeInterval time interval between epochs expressed in seconds (s).
324 * @param oldPosition previous body position with respect the Earth, resolved about
325 * north, east and down axes.
326 * @param oldVn previous velocity of body with respect the Earth, resolved about
327 * north-axis.
328 * @param oldVe previous velocity of body with respect the Earth, resolved about
329 * east-axis.
330 * @param oldVd previous velocity of body with respect the Earth, resolved about
331 * down-axis.
332 * @param vn current velocity of body with respect the Earth, resolved about
333 * north-axis.
334 * @param ve current velocity of body with respect the Earth, resolved about
335 * east-axis.
336 * @param vd current velocity of body with respect the Earth, resolved about
337 * down-axis.
338 * @param result instance where updated curvilinear position will be stored.
339 * @throws IllegalArgumentException if provided time interval is negative.
340 */
341 public void estimate(
342 final double timeInterval, final NEDPosition oldPosition,
343 final Speed oldVn, final Speed oldVe, final Speed oldVd, final Speed vn, final Speed ve, final Speed vd,
344 final NEDPosition result) {
345 estimatePosition(timeInterval, oldPosition, oldVn, oldVe, oldVd, vn, ve, vd, result);
346 }
347
348 /**
349 * Estimates curvilinear position by integrating the velocity.
350 *
351 * @param timeInterval time interval between epochs.
352 * @param oldPosition previous body position with respect the Earth, resolved about
353 * north, east and down axes.
354 * @param oldVn previous velocity of body with respect the Earth, resolved about
355 * north-axis.
356 * @param oldVe previous velocity of body with respect the Earth, resolved about
357 * east-axis.
358 * @param oldVd previous velocity of body with respect the Earth, resolved about
359 * down-axis.
360 * @param vn current velocity of body with respect the Earth, resolved about
361 * north-axis.
362 * @param ve current velocity of body with respect the Earth, resolved about
363 * east-axis.
364 * @param vd current velocity of body with respect the Earth, resolved about
365 * down-axis.
366 * @param result instance where updated curvilinear position will be stored.
367 * @throws IllegalArgumentException if provided time interval is negative.
368 */
369 public void estimate(
370 final Time timeInterval, final NEDPosition oldPosition,
371 final Speed oldVn, final Speed oldVe, final Speed oldVd, final Speed vn, final Speed ve, final Speed vd,
372 final NEDPosition result) {
373 estimatePosition(timeInterval, oldPosition, oldVn, oldVe, oldVd, vn, ve, vd, result);
374 }
375
376 /**
377 * Estimates curvilinear position by integrating the velocity.
378 *
379 * @param timeInterval time interval between epochs expressed in seconds (s).
380 * @param oldLatitude previous latitude expressed in radians (rad).
381 * @param oldLongitude previous longitude expressed in radians (rad).
382 * @param oldHeight previous height expressed in meters (m).
383 * @param oldVelocity previous velocity of body with respect the Earth, resolved
384 * about north, east and down axes and expressed in meters per
385 * second (m/s).
386 * @param velocity current velocity of body with respect the Earth, resolved
387 * about north, east and down axes and expressed in meters per
388 * second (m/s).
389 * @param result instance where updated curvilinear position will be stored.
390 * @throws IllegalArgumentException if provided time interval is negative.
391 */
392 public void estimate(
393 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
394 final NEDVelocity oldVelocity, final NEDVelocity velocity, final NEDPosition result) {
395 estimatePosition(timeInterval, oldLatitude, oldLongitude, oldHeight, oldVelocity, velocity, result);
396 }
397
398 /**
399 * Estimates curvilinear position by integrating the velocity.
400 *
401 * @param timeInterval time interval between epochs.
402 * @param oldLatitude previous latitude expressed in radians (rad).
403 * @param oldLongitude previous longitude expressed in radians (rad).
404 * @param oldHeight previous height expressed in meters (m).
405 * @param oldVelocity previous velocity of body with respect the Earth, resolved
406 * about north, east and down axes and expressed in meters per
407 * second (m/s).
408 * @param velocity current velocity of body with respect the Earth, resolved
409 * about north, east and down axes and expressed in meters per
410 * second (m/s).
411 * @param result instance where updated curvilinear position will be stored.
412 * @throws IllegalArgumentException if provided time interval is negative.
413 */
414 public void estimate(
415 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
416 final NEDVelocity oldVelocity, final NEDVelocity velocity, final NEDPosition result) {
417 estimatePosition(timeInterval, oldLatitude, oldLongitude, oldHeight, oldVelocity, velocity, result);
418 }
419
420 /**
421 * Estimates velocity from curvilinear position changes and taking into account previous
422 * velocity respect NED frame.
423 *
424 * @param timeInterval time interval between epochs expressed in seconds (s).
425 * @param oldLatitude previous latitude.
426 * @param oldLongitude previous longitude.
427 * @param oldHeight previous height.
428 * @param oldVelocity previous velocity of body with respect the Earth, resolved
429 * about north, east and down axes and expressed in meters per
430 * second (m/s).
431 * @param velocity current velocity of body with respect the Earth, resolved
432 * about north, east and down axes and expressed in meters per
433 * second (m/s).
434 * @param result instance where updated curvilinear position will be stored.
435 * @throws IllegalArgumentException if provided time interval is negative.
436 */
437 public void estimate(
438 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
439 final NEDVelocity oldVelocity, final NEDVelocity velocity, final NEDPosition result) {
440 estimatePosition(timeInterval, oldLatitude, oldLongitude, oldHeight, oldVelocity, velocity, result);
441 }
442
443 /**
444 * Estimates velocity from curvilinear position changes and taking into account previous
445 * velocity respect NED frame.
446 *
447 * @param timeInterval time interval between epochs.
448 * @param oldLatitude previous latitude.
449 * @param oldLongitude previous longitude.
450 * @param oldHeight previous height.
451 * @param oldVelocity previous velocity of body with respect the Earth, resolved
452 * about north, east and down axes and expressed in meters per
453 * second (m/s).
454 * @param velocity current velocity of body with respect the Earth, resolved
455 * about north, east and down axes and expressed in meters per
456 * second (m/s).
457 * @param result instance where updated curvilinear position will be stored.
458 * @throws IllegalArgumentException if provided time interval is negative.
459 */
460 public void estimate(
461 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
462 final NEDVelocity oldVelocity, final NEDVelocity velocity, final NEDPosition result) {
463 estimatePosition(timeInterval, oldLatitude, oldLongitude, oldHeight, oldVelocity, velocity, result);
464 }
465
466 /**
467 * Estimates curvilinear position by integrating the velocity.
468 *
469 * @param timeInterval time interval between epochs expressed in seconds (s).
470 * @param oldPosition previous body position with respect the Earth, resolved about
471 * north, east and down axes.
472 * @param oldVelocity previous velocity of body with respect the Earth, resolved
473 * about north, east and down axes and expressed in meters per
474 * second (m/s).
475 * @param velocity current velocity of body with respect the Earth, resolved
476 * about north, east and down axes and expressed in meters per
477 * second (m/s).
478 * @param result instance where updated curvilinear position will be stored.
479 * @throws IllegalArgumentException if provided time interval is negative.
480 */
481 public void estimate(
482 final double timeInterval, final NEDPosition oldPosition, final NEDVelocity oldVelocity,
483 final NEDVelocity velocity, final NEDPosition result) {
484 estimatePosition(timeInterval, oldPosition, oldVelocity, velocity, result);
485 }
486
487 /**
488 * Estimates curvilinear position by integrating the velocity.
489 *
490 * @param timeInterval time interval between epochs.
491 * @param oldPosition previous body position with respect the Earth, resolved about
492 * north, east and down axes.
493 * @param oldVelocity previous velocity of body with respect the Earth, resolved
494 * about north, east and down axes and expressed in meters per
495 * second (m/s).
496 * @param velocity current velocity of body with respect the Earth, resolved
497 * about north, east and down axes and expressed in meters per
498 * second (m/s).
499 * @param result instance where updated curvilinear position will be stored.
500 * @throws IllegalArgumentException if provided time interval is negative.
501 */
502 public void estimate(
503 final Time timeInterval, final NEDPosition oldPosition, final NEDVelocity oldVelocity,
504 final NEDVelocity velocity, final NEDPosition result) {
505 estimatePosition(timeInterval, oldPosition, oldVelocity, velocity, result);
506 }
507
508 /**
509 * Estimates curvilinear position by integrating the velocity.
510 *
511 * @param timeInterval time interval between epochs expressed in seconds (s).
512 * @param oldLatitude previous latitude expressed in radians (rad).
513 * @param oldLongitude previous longitude expressed in radians (rad).
514 * @param oldHeight previous height expressed in meters (m).
515 * @param oldVn previous velocity of body with respect the Earth, resolved about
516 * north-axis and expressed in meters per second (m/s).
517 * @param oldVe previous velocity of body with respect the Earth, resolved about
518 * east-axis and expressed in meters per second (m/s).
519 * @param oldVd previous velocity of body with respect the Earth, resolved about
520 * down-axis and expressed in meters per second (m/s).
521 * @param vn current velocity of body with respect the Earth, resolved about
522 * north-axis and expressed in meters per second (m/s).
523 * @param ve current velocity of body with respect the Earth, resolved about
524 * east-axis and expressed in meters per second (m/s).
525 * @param vd current velocity of body with respect the Earth, resolved about
526 * down-axis and expressed in meters per second (m/s).
527 * @return new updated curvilinear position.
528 * @throws IllegalArgumentException if provided time interval is negative.
529 */
530 public NEDPosition estimateAndReturnNew(
531 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
532 final double oldVn, final double oldVe, final double oldVd,
533 final double vn, final double ve, final double vd) {
534 return estimatePositionAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldVn, oldVe, oldVd,
535 vn, ve, vd);
536 }
537
538 /**
539 * Estimates curvilinear position by integrating the velocity.
540 *
541 * @param timeInterval time interval between epochs.
542 * @param oldLatitude previous latitude expressed in radians (rad).
543 * @param oldLongitude previous longitude expressed in radians (rad).
544 * @param oldHeight previous height expressed in meters (m).
545 * @param oldVn previous velocity of body with respect the Earth, resolved about
546 * north-axis and expressed in meters per second (m/s).
547 * @param oldVe previous velocity of body with respect the Earth, resolved about
548 * east-axis and expressed in meters per second (m/s).
549 * @param oldVd previous velocity of body with respect the Earth, resolved about
550 * down-axis and expressed in meters per second (m/s).
551 * @param vn current velocity of body with respect the Earth, resolved about
552 * north-axis and expressed in meters per second (m/s).
553 * @param ve current velocity of body with respect the Earth, resolved about
554 * east-axis and expressed in meters per second (m/s).
555 * @param vd current velocity of body with respect the Earth, resolved about
556 * down-axis and expressed in meters per second (m/s).
557 * @return new updated curvilinear position.
558 * @throws IllegalArgumentException if provided time interval is negative.
559 */
560 public NEDPosition estimateAndReturnNew(
561 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
562 final double oldVn, final double oldVe, final double oldVd,
563 final double vn, final double ve, final double vd) {
564 return estimatePositionAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldVn, oldVe, oldVd,
565 vn, ve, vd);
566 }
567
568 /**
569 * Estimates curvilinear position by integrating the velocity.
570 *
571 * @param timeInterval time interval between epochs expressed in seconds (s).
572 * @param oldLatitude previous latitude.
573 * @param oldLongitude previous longitude.
574 * @param oldHeight previous height.
575 * @param oldVn previous velocity of body with respect the Earth, resolved about
576 * north-axis.
577 * @param oldVe previous velocity of body with respect the Earth, resolved about
578 * east-axis.
579 * @param oldVd previous velocity of body with respect the Earth, resolved about
580 * down-axis.
581 * @param vn current velocity of body with respect the Earth, resolved about
582 * north-axis.
583 * @param ve current velocity of body with respect the Earth, resolved about
584 * east-axis.
585 * @param vd current velocity of body with respect the Earth, resolved about
586 * down-axis.
587 * @return new updated curvilinear position.
588 * @throws IllegalArgumentException if provided time interval is negative.
589 */
590 public NEDPosition estimateAndReturnNew(
591 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
592 final Speed oldVn, final Speed oldVe, final Speed oldVd, final Speed vn, final Speed ve, final Speed vd) {
593 return estimatePositionAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldVn, oldVe, oldVd,
594 vn, ve, vd);
595 }
596
597 /**
598 * Estimates curvilinear position by integrating the velocity.
599 *
600 * @param timeInterval time interval between epochs.
601 * @param oldLatitude previous latitude.
602 * @param oldLongitude previous longitude.
603 * @param oldHeight previous height.
604 * @param oldVn previous velocity of body with respect the Earth, resolved about
605 * north-axis.
606 * @param oldVe previous velocity of body with respect the Earth, resolved about
607 * east-axis.
608 * @param oldVd previous velocity of body with respect the Earth, resolved about
609 * down-axis.
610 * @param vn current velocity of body with respect the Earth, resolved about
611 * north-axis.
612 * @param ve current velocity of body with respect the Earth, resolved about
613 * east-axis.
614 * @param vd current velocity of body with respect the Earth, resolved about
615 * down-axis.
616 * @return new updated curvilinear position.
617 * @throws IllegalArgumentException if provided time interval is negative.
618 */
619 public NEDPosition estimateAndReturnNew(
620 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
621 final Speed oldVn, final Speed oldVe, final Speed oldVd, final Speed vn, final Speed ve, final Speed vd) {
622 return estimatePositionAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldVn, oldVe, oldVd,
623 vn, ve, vd);
624 }
625
626 /**
627 * Estimates curvilinear position by integrating the velocity.
628 *
629 * @param timeInterval time interval between epochs expressed in seconds (s).
630 * @param oldLatitude previous latitude.
631 * @param oldLongitude previous longitude.
632 * @param oldHeight previous height.
633 * @param oldVn previous velocity of body with respect the Earth, resolved about
634 * north-axis and expressed in meters per second (m/s).
635 * @param oldVe previous velocity of body with respect the Earth, resolved about
636 * east-axis and expressed in meters per second (m/s).
637 * @param oldVd previous velocity of body with respect the Earth, resolved about
638 * down-axis and expressed in meters per second (m/s).
639 * @param vn current velocity of body with respect the Earth, resolved about
640 * north-axis and expressed in meters per second (m/s).
641 * @param ve current velocity of body with respect the Earth, resolved about
642 * east-axis and expressed in meters per second (m/s).
643 * @param vd current velocity of body with respect the Earth, resolved about
644 * down-axis and expressed in meters per second (m/s).
645 * @return new updated curvilinear position.
646 * @throws IllegalArgumentException if provided time interval is negative.
647 */
648 public NEDPosition estimateAndReturnNew(
649 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
650 final double oldVn, final double oldVe, final double oldVd,
651 final double vn, final double ve, final double vd) {
652 return estimatePositionAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldVn, oldVe, oldVd,
653 vn, ve, vd);
654 }
655
656 /**
657 * Estimates curvilinear position by integrating the velocity.
658 *
659 * @param timeInterval time interval between epochs.
660 * @param oldLatitude previous latitude.
661 * @param oldLongitude previous longitude.
662 * @param oldHeight previous height.
663 * @param oldVn previous velocity of body with respect the Earth, resolved about
664 * north-axis and expressed in meters per second (m/s).
665 * @param oldVe previous velocity of body with respect the Earth, resolved about
666 * east-axis and expressed in meters per second (m/s).
667 * @param oldVd previous velocity of body with respect the Earth, resolved about
668 * down-axis and expressed in meters per second (m/s).
669 * @param vn current velocity of body with respect the Earth, resolved about
670 * north-axis and expressed in meters per second (m/s).
671 * @param ve current velocity of body with respect the Earth, resolved about
672 * east-axis and expressed in meters per second (m/s).
673 * @param vd current velocity of body with respect the Earth, resolved about
674 * down-axis and expressed in meters per second (m/s).
675 * @return new updated curvilinear position.
676 * @throws IllegalArgumentException if provided time interval is negative.
677 */
678 public NEDPosition estimateAndReturnNew(
679 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
680 final double oldVn, final double oldVe, final double oldVd,
681 final double vn, final double ve, final double vd) {
682 return estimatePositionAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldVn, oldVe, oldVd,
683 vn, ve, vd);
684 }
685
686 /**
687 * Estimates curvilinear position by integrating the velocity.
688 *
689 * @param timeInterval time interval between epochs expressed in seconds (s).
690 * @param oldLatitude previous latitude expressed in radians (rad).
691 * @param oldLongitude previous longitude expressed in radians (rad).
692 * @param oldHeight previous height expressed in meters (m).
693 * @param oldVn previous velocity of body with respect the Earth, resolved about
694 * north-axis.
695 * @param oldVe previous velocity of body with respect the Earth, resolved about
696 * east-axis.
697 * @param oldVd previous velocity of body with respect the Earth, resolved about
698 * down-axis.
699 * @param vn current velocity of body with respect the Earth, resolved about
700 * north-axis.
701 * @param ve current velocity of body with respect the Earth, resolved about
702 * east-axis.
703 * @param vd current velocity of body with respect the Earth, resolved about
704 * down-axis.
705 * @return new updated curvilinear position.
706 * @throws IllegalArgumentException if provided time interval is negative.
707 */
708 public NEDPosition estimateAndReturnNew(
709 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
710 final Speed oldVn, final Speed oldVe, final Speed oldVd, final Speed vn, final Speed ve, final Speed vd) {
711 return estimatePositionAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldVn, oldVe, oldVd,
712 vn, ve, vd);
713 }
714
715 /**
716 * Estimates curvilinear position by integrating the velocity.
717 *
718 * @param timeInterval time interval between epochs.
719 * @param oldLatitude previous latitude expressed in radians (rad).
720 * @param oldLongitude previous longitude expressed in radians (rad).
721 * @param oldHeight previous height expressed in meters (m).
722 * @param oldVn previous velocity of body with respect the Earth, resolved about
723 * north-axis.
724 * @param oldVe previous velocity of body with respect the Earth, resolved about
725 * east-axis.
726 * @param oldVd previous velocity of body with respect the Earth, resolved about
727 * down-axis.
728 * @param vn current velocity of body with respect the Earth, resolved about
729 * north-axis.
730 * @param ve current velocity of body with respect the Earth, resolved about
731 * east-axis.
732 * @param vd current velocity of body with respect the Earth, resolved about
733 * down-axis.
734 * @return new updated curvilinear position.
735 * @throws IllegalArgumentException if provided time interval is negative.
736 */
737 public NEDPosition estimateAndReturnNew(
738 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
739 final Speed oldVn, final Speed oldVe, final Speed oldVd, final Speed vn, final Speed ve, final Speed vd) {
740 return estimatePositionAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldVn, oldVe, oldVd,
741 vn, ve, vd);
742 }
743
744 /**
745 * Estimates curvilinear position by integrating the velocity.
746 *
747 * @param timeInterval time interval between epochs expressed in seconds (s).
748 * @param oldPosition previous body position with respect the Earth, resolved about
749 * north, east and down axes.
750 * @param oldVn previous velocity of body with respect the Earth, resolved about
751 * north-axis and expressed in meters per second (m/s).
752 * @param oldVe previous velocity of body with respect the Earth, resolved about
753 * east-axis and expressed in meters per second (m/s).
754 * @param oldVd previous velocity of body with respect the Earth, resolved about
755 * down-axis and expressed in meters per second (m/s).
756 * @param vn current velocity of body with respect the Earth, resolved about
757 * north-axis and expressed in meters per second (m/s).
758 * @param ve current velocity of body with respect the Earth, resolved about
759 * east-axis and expressed in meters per second (m/s).
760 * @param vd current velocity of body with respect the Earth, resolved about
761 * down-axis and expressed in meters per second (m/s).
762 * @return new updated curvilinear position.
763 * @throws IllegalArgumentException if provided time interval is negative.
764 */
765 public NEDPosition estimateAndReturnNew(
766 final double timeInterval, final NEDPosition oldPosition,
767 final double oldVn, final double oldVe, final double oldVd,
768 final double vn, final double ve, final double vd) {
769 return estimatePositionAndReturnNew(timeInterval, oldPosition, oldVn, oldVe, oldVd, vn, ve, vd);
770 }
771
772 /**
773 * Estimates curvilinear position by integrating the velocity.
774 *
775 * @param timeInterval time interval between epochs.
776 * @param oldPosition previous body position with respect the Earth, resolved about
777 * north, east and down axes.
778 * @param oldVn previous velocity of body with respect the Earth, resolved about
779 * north-axis and expressed in meters per second (m/s).
780 * @param oldVe previous velocity of body with respect the Earth, resolved about
781 * east-axis and expressed in meters per second (m/s).
782 * @param oldVd previous velocity of body with respect the Earth, resolved about
783 * down-axis and expressed in meters per second (m/s).
784 * @param vn current velocity of body with respect the Earth, resolved about
785 * north-axis and expressed in meters per second (m/s).
786 * @param ve current velocity of body with respect the Earth, resolved about
787 * east-axis and expressed in meters per second (m/s).
788 * @param vd current velocity of body with respect the Earth, resolved about
789 * down-axis and expressed in meters per second (m/s).
790 * @return new updated curvilinear position.
791 * @throws IllegalArgumentException if provided time interval is negative.
792 */
793 public NEDPosition estimateAndReturnNew(
794 final Time timeInterval, final NEDPosition oldPosition,
795 final double oldVn, final double oldVe, final double oldVd,
796 final double vn, final double ve, final double vd) {
797 return estimatePositionAndReturnNew(timeInterval, oldPosition, oldVn, oldVe, oldVd, vn, ve, vd);
798 }
799
800 /**
801 * Estimates curvilinear position by integrating the velocity.
802 *
803 * @param timeInterval time interval between epochs expressed in seconds (s).
804 * @param oldPosition previous body position with respect the Earth, resolved about
805 * north, east and down axes.
806 * @param oldVn previous velocity of body with respect the Earth, resolved about
807 * north-axis.
808 * @param oldVe previous velocity of body with respect the Earth, resolved about
809 * east-axis.
810 * @param oldVd previous velocity of body with respect the Earth, resolved about
811 * down-axis.
812 * @param vn current velocity of body with respect the Earth, resolved about
813 * north-axis.
814 * @param ve current velocity of body with respect the Earth, resolved about
815 * east-axis.
816 * @param vd current velocity of body with respect the Earth, resolved about
817 * down-axis.
818 * @return new updated curvilinear position.
819 * @throws IllegalArgumentException if provided time interval is negative.
820 */
821 public NEDPosition estimateAndReturnNew(
822 final double timeInterval, final NEDPosition oldPosition,
823 final Speed oldVn, final Speed oldVe, final Speed oldVd, final Speed vn, final Speed ve, final Speed vd) {
824 return estimatePositionAndReturnNew(timeInterval, oldPosition, oldVn, oldVe, oldVd, vn, ve, vd);
825 }
826
827 /**
828 * Estimates curvilinear position by integrating the velocity.
829 *
830 * @param timeInterval time interval between epochs.
831 * @param oldPosition previous body position with respect the Earth, resolved about
832 * north, east and down axes.
833 * @param oldVn previous velocity of body with respect the Earth, resolved about
834 * north-axis.
835 * @param oldVe previous velocity of body with respect the Earth, resolved about
836 * east-axis.
837 * @param oldVd previous velocity of body with respect the Earth, resolved about
838 * down-axis.
839 * @param vn current velocity of body with respect the Earth, resolved about
840 * north-axis.
841 * @param ve current velocity of body with respect the Earth, resolved about
842 * east-axis.
843 * @param vd current velocity of body with respect the Earth, resolved about
844 * down-axis.
845 * @return new updated curvilinear position.
846 * @throws IllegalArgumentException if provided time interval is negative.
847 */
848 public NEDPosition estimateAndReturnNew(
849 final Time timeInterval, final NEDPosition oldPosition,
850 final Speed oldVn, final Speed oldVe, final Speed oldVd, final Speed vn, final Speed ve, final Speed vd) {
851 return estimatePositionAndReturnNew(timeInterval, oldPosition, oldVn, oldVe, oldVd, vn, ve, vd);
852 }
853
854 /**
855 * Estimates curvilinear position by integrating the velocity.
856 *
857 * @param timeInterval time interval between epochs expressed in seconds (s).
858 * @param oldLatitude previous latitude expressed in radians (rad).
859 * @param oldLongitude previous longitude expressed in radians (rad).
860 * @param oldHeight previous height expressed in meters (m).
861 * @param oldVelocity previous velocity of body with respect the Earth, resolved
862 * about north, east and down axes and expressed in meters per
863 * second (m/s).
864 * @param velocity current velocity of body with respect the Earth, resolved
865 * about north, east and down axes and expressed in meters per
866 * second (m/s).
867 * @return new updated curvilinear position.
868 * @throws IllegalArgumentException if provided time interval is negative.
869 */
870 public NEDPosition estimateAndReturnNew(
871 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
872 final NEDVelocity oldVelocity, final NEDVelocity velocity) {
873 return estimatePositionAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldVelocity, velocity);
874 }
875
876 /**
877 * Estimates curvilinear position by integrating the velocity.
878 *
879 * @param timeInterval time interval between epochs.
880 * @param oldLatitude previous latitude expressed in radians (rad).
881 * @param oldLongitude previous longitude expressed in radians (rad).
882 * @param oldHeight previous height expressed in meters (m).
883 * @param oldVelocity previous velocity of body with respect the Earth, resolved
884 * about north, east and down axes and expressed in meters per
885 * second (m/s).
886 * @param velocity current velocity of body with respect the Earth, resolved
887 * about north, east and down axes and expressed in meters per
888 * second (m/s).
889 * @return new updated curvilinear position.
890 * @throws IllegalArgumentException if provided time interval is negative.
891 */
892 public NEDPosition estimateAndReturnNew(
893 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
894 final NEDVelocity oldVelocity, final NEDVelocity velocity) {
895 return estimatePositionAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldVelocity, velocity);
896 }
897
898 /**
899 * Estimates velocity from curvilinear position changes and taking into account previous
900 * velocity respect NED frame.
901 *
902 * @param timeInterval time interval between epochs expressed in seconds (s).
903 * @param oldLatitude previous latitude.
904 * @param oldLongitude previous longitude.
905 * @param oldHeight previous height.
906 * @param oldVelocity previous velocity of body with respect the Earth, resolved
907 * about north, east and down axes and expressed in meters per
908 * second (m/s).
909 * @param velocity current velocity of body with respect the Earth, resolved
910 * about north, east and down axes and expressed in meters per
911 * second (m/s).
912 * @return new updated curvilinear position.
913 * @throws IllegalArgumentException if provided time interval is negative.
914 */
915 public NEDPosition estimateAndReturnNew(
916 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
917 final NEDVelocity oldVelocity, final NEDVelocity velocity) {
918 return estimatePositionAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldVelocity, velocity);
919 }
920
921 /**
922 * Estimates velocity from curvilinear position changes and taking into account previous
923 * velocity respect NED frame.
924 *
925 * @param timeInterval time interval between epochs.
926 * @param oldLatitude previous latitude.
927 * @param oldLongitude previous longitude.
928 * @param oldHeight previous height.
929 * @param oldVelocity previous velocity of body with respect the Earth, resolved
930 * about north, east and down axes and expressed in meters per
931 * second (m/s).
932 * @param velocity current velocity of body with respect the Earth, resolved
933 * about north, east and down axes and expressed in meters per
934 * second (m/s).
935 * @return new updated curvilinear position.
936 * @throws IllegalArgumentException if provided time interval is negative.
937 */
938 public NEDPosition estimateAndReturnNew(
939 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
940 final NEDVelocity oldVelocity, final NEDVelocity velocity) {
941 return estimatePositionAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldVelocity, velocity);
942 }
943
944 /**
945 * Estimates curvilinear position by integrating the velocity.
946 *
947 * @param timeInterval time interval between epochs expressed in seconds (s).
948 * @param oldPosition previous body position with respect the Earth, resolved about
949 * north, east and down axes.
950 * @param oldVelocity previous velocity of body with respect the Earth, resolved
951 * about north, east and down axes and expressed in meters per
952 * second (m/s).
953 * @param velocity current velocity of body with respect the Earth, resolved
954 * about north, east and down axes and expressed in meters per
955 * second (m/s).
956 * @return new updated curvilinear position.
957 * @throws IllegalArgumentException if provided time interval is negative.
958 */
959 public NEDPosition estimateAndReturnNew(
960 final double timeInterval, final NEDPosition oldPosition, final NEDVelocity oldVelocity,
961 final NEDVelocity velocity) {
962 return estimatePositionAndReturnNew(timeInterval, oldPosition, oldVelocity, velocity);
963 }
964
965 /**
966 * Estimates curvilinear position by integrating the velocity.
967 *
968 * @param timeInterval time interval between epochs.
969 * @param oldPosition previous body position with respect the Earth, resolved about
970 * north, east and down axes.
971 * @param oldVelocity previous velocity of body with respect the Earth, resolved
972 * about north, east and down axes and expressed in meters per
973 * second (m/s).
974 * @param velocity current velocity of body with respect the Earth, resolved
975 * about north, east and down axes and expressed in meters per
976 * second (m/s).
977 * @return new updated curvilinear position.
978 * @throws IllegalArgumentException if provided time interval is negative.
979 */
980 public NEDPosition estimateAndReturnNew(
981 final Time timeInterval, final NEDPosition oldPosition, final NEDVelocity oldVelocity,
982 final NEDVelocity velocity) {
983 return estimatePositionAndReturnNew(timeInterval, oldPosition, oldVelocity, velocity);
984 }
985
986 /**
987 * Estimates curvilinear position by integrating the velocity.
988 *
989 * @param timeInterval time interval between epochs expressed in seconds (s).
990 * @param oldLatitude previous latitude expressed in radians (rad).
991 * @param oldLongitude previous longitude expressed in radians (rad).
992 * @param oldHeight previous height expressed in meters (m).
993 * @param oldVn previous velocity of body with respect the Earth, resolved about
994 * north-axis and expressed in meters per second (m/s).
995 * @param oldVe previous velocity of body with respect the Earth, resolved about
996 * east-axis and expressed in meters per second (m/s).
997 * @param oldVd previous velocity of body with respect the Earth, resolved about
998 * down-axis and expressed in meters per second (m/s).
999 * @param vn current velocity of body with respect the Earth, resolved about
1000 * north-axis and expressed in meters per second (m/s).
1001 * @param ve current velocity of body with respect the Earth, resolved about
1002 * east-axis and expressed in meters per second (m/s).
1003 * @param vd current velocity of body with respect the Earth, resolved about
1004 * down-axis and expressed in meters per second (m/s).
1005 * @param result instance where updated curvilinear position will be stored.
1006 * @throws IllegalArgumentException if provided time interval is negative.
1007 */
1008 public static void estimatePosition(
1009 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
1010 final double oldVn, final double oldVe, final double oldVd,
1011 final double vn, final double ve, final double vd, final NEDPosition result) {
1012
1013 if (timeInterval < 0.0) {
1014 throw new IllegalArgumentException();
1015 }
1016
1017 // Calculate meridian and transverse radii of curvature
1018 final var oldRadii = RadiiOfCurvatureEstimator.estimateRadiiOfCurvatureAndReturnNew(oldLatitude);
1019 final var oldRn = oldRadii.getRn();
1020 final var oldRe = oldRadii.getRe();
1021
1022 // Update height using (5.56)
1023 final var height = oldHeight - 0.5 * timeInterval * (oldVd + vd);
1024
1025 // Update latitude using (5.56)
1026 final var latitude = oldLatitude + 0.5 * timeInterval * (oldVn / (oldRn + oldHeight)
1027 + vn / (oldRn + height));
1028
1029 // Calculate meridian and transverse radii of curvature
1030 final var radii = RadiiOfCurvatureEstimator.estimateRadiiOfCurvatureAndReturnNew(latitude);
1031 final var re = radii.getRe();
1032
1033 // Update longitude using (5.56)
1034 final var longitude = oldLongitude
1035 + 0.5 * timeInterval * (oldVe / ((oldRe + oldHeight) * Math.cos(oldLatitude))
1036 + ve / ((re + height) * Math.cos(latitude)));
1037
1038 result.setCoordinates(latitude, longitude, height);
1039 }
1040
1041 /**
1042 * Estimates curvilinear position by integrating the velocity.
1043 *
1044 * @param timeInterval time interval between epochs.
1045 * @param oldLatitude previous latitude expressed in radians (rad).
1046 * @param oldLongitude previous longitude expressed in radians (rad).
1047 * @param oldHeight previous height expressed in meters (m).
1048 * @param oldVn previous velocity of body with respect the Earth, resolved about
1049 * north-axis and expressed in meters per second (m/s).
1050 * @param oldVe previous velocity of body with respect the Earth, resolved about
1051 * east-axis and expressed in meters per second (m/s).
1052 * @param oldVd previous velocity of body with respect the Earth, resolved about
1053 * down-axis and expressed in meters per second (m/s).
1054 * @param vn current velocity of body with respect the Earth, resolved about
1055 * north-axis and expressed in meters per second (m/s).
1056 * @param ve current velocity of body with respect the Earth, resolved about
1057 * east-axis and expressed in meters per second (m/s).
1058 * @param vd current velocity of body with respect the Earth, resolved about
1059 * down-axis and expressed in meters per second (m/s).
1060 * @param result instance where updated curvilinear position will be stored.
1061 * @throws IllegalArgumentException if provided time interval is negative.
1062 */
1063 public static void estimatePosition(
1064 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
1065 final double oldVn, final double oldVe, final double oldVd,
1066 final double vn, final double ve, final double vd, final NEDPosition result) {
1067 estimatePosition(convertTimeToDouble(timeInterval), oldLatitude, oldLongitude, oldHeight, oldVn, oldVe, oldVd,
1068 vn, ve, vd, result);
1069 }
1070
1071 /**
1072 * Estimates curvilinear position by integrating the velocity.
1073 *
1074 * @param timeInterval time interval between epochs expressed in seconds (s).
1075 * @param oldLatitude previous latitude.
1076 * @param oldLongitude previous longitude.
1077 * @param oldHeight previous height.
1078 * @param oldVn previous velocity of body with respect the Earth, resolved about
1079 * north-axis.
1080 * @param oldVe previous velocity of body with respect the Earth, resolved about
1081 * east-axis.
1082 * @param oldVd previous velocity of body with respect the Earth, resolved about
1083 * down-axis.
1084 * @param vn current velocity of body with respect the Earth, resolved about
1085 * north-axis.
1086 * @param ve current velocity of body with respect the Earth, resolved about
1087 * east-axis.
1088 * @param vd current velocity of body with respect the Earth, resolved about
1089 * down-axis.
1090 * @param result instance where updated curvilinear position will be stored.
1091 * @throws IllegalArgumentException if provided time interval is negative.
1092 */
1093 public static void estimatePosition(
1094 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
1095 final Speed oldVn, final Speed oldVe, final Speed oldVd, final Speed vn, final Speed ve, final Speed vd,
1096 final NEDPosition result) {
1097 estimatePosition(timeInterval, convertAngleToDouble(oldLatitude), convertAngleToDouble(oldLongitude),
1098 convertDistanceToDouble(oldHeight), convertSpeedToDouble(oldVn), convertSpeedToDouble(oldVe),
1099 convertSpeedToDouble(oldVd), convertSpeedToDouble(vn), convertSpeedToDouble(ve),
1100 convertSpeedToDouble(vd), result);
1101 }
1102
1103 /**
1104 * Estimates curvilinear position by integrating the velocity.
1105 *
1106 * @param timeInterval time interval between epochs.
1107 * @param oldLatitude previous latitude.
1108 * @param oldLongitude previous longitude.
1109 * @param oldHeight previous height.
1110 * @param oldVn previous velocity of body with respect the Earth, resolved about
1111 * north-axis.
1112 * @param oldVe previous velocity of body with respect the Earth, resolved about
1113 * east-axis.
1114 * @param oldVd previous velocity of body with respect the Earth, resolved about
1115 * down-axis.
1116 * @param vn current velocity of body with respect the Earth, resolved about
1117 * north-axis.
1118 * @param ve current velocity of body with respect the Earth, resolved about
1119 * east-axis.
1120 * @param vd current velocity of body with respect the Earth, resolved about
1121 * down-axis.
1122 * @param result instance where updated curvilinear position will be stored.
1123 * @throws IllegalArgumentException if provided time interval is negative.
1124 */
1125 public static void estimatePosition(
1126 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
1127 final Speed oldVn, final Speed oldVe, final Speed oldVd, final Speed vn, final Speed ve, final Speed vd,
1128 final NEDPosition result) {
1129 estimatePosition(convertTimeToDouble(timeInterval), oldLatitude, oldLongitude, oldHeight, oldVn, oldVe, oldVd,
1130 vn, ve, vd, result);
1131 }
1132
1133 /**
1134 * Estimates curvilinear position by integrating the velocity.
1135 *
1136 * @param timeInterval time interval between epochs expressed in seconds (s).
1137 * @param oldLatitude previous latitude.
1138 * @param oldLongitude previous longitude.
1139 * @param oldHeight previous height.
1140 * @param oldVn previous velocity of body with respect the Earth, resolved about
1141 * north-axis and expressed in meters per second (m/s).
1142 * @param oldVe previous velocity of body with respect the Earth, resolved about
1143 * east-axis and expressed in meters per second (m/s).
1144 * @param oldVd previous velocity of body with respect the Earth, resolved about
1145 * down-axis and expressed in meters per second (m/s).
1146 * @param vn current velocity of body with respect the Earth, resolved about
1147 * north-axis and expressed in meters per second (m/s).
1148 * @param ve current velocity of body with respect the Earth, resolved about
1149 * east-axis and expressed in meters per second (m/s).
1150 * @param vd current velocity of body with respect the Earth, resolved about
1151 * down-axis and expressed in meters per second (m/s).
1152 * @param result instance where updated curvilinear position will be stored.
1153 * @throws IllegalArgumentException if provided time interval is negative.
1154 */
1155 public static void estimatePosition(
1156 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
1157 final double oldVn, final double oldVe, final double oldVd,
1158 final double vn, final double ve, final double vd, final NEDPosition result) {
1159 estimatePosition(timeInterval, convertAngleToDouble(oldLatitude), convertAngleToDouble(oldLongitude),
1160 convertDistanceToDouble(oldHeight), oldVn, oldVe, oldVd, vn, ve, vd, result);
1161 }
1162
1163 /**
1164 * Estimates curvilinear position by integrating the velocity.
1165 *
1166 * @param timeInterval time interval between epochs.
1167 * @param oldLatitude previous latitude.
1168 * @param oldLongitude previous longitude.
1169 * @param oldHeight previous height.
1170 * @param oldVn previous velocity of body with respect the Earth, resolved about
1171 * north-axis and expressed in meters per second (m/s).
1172 * @param oldVe previous velocity of body with respect the Earth, resolved about
1173 * east-axis and expressed in meters per second (m/s).
1174 * @param oldVd previous velocity of body with respect the Earth, resolved about
1175 * down-axis and expressed in meters per second (m/s).
1176 * @param vn current velocity of body with respect the Earth, resolved about
1177 * north-axis and expressed in meters per second (m/s).
1178 * @param ve current velocity of body with respect the Earth, resolved about
1179 * east-axis and expressed in meters per second (m/s).
1180 * @param vd current velocity of body with respect the Earth, resolved about
1181 * down-axis and expressed in meters per second (m/s).
1182 * @param result instance where updated curvilinear position will be stored.
1183 * @throws IllegalArgumentException if provided time interval is negative.
1184 */
1185 public static void estimatePosition(
1186 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
1187 final double oldVn, final double oldVe, final double oldVd,
1188 final double vn, final double ve, final double vd, final NEDPosition result) {
1189 estimatePosition(convertTimeToDouble(timeInterval), oldLatitude, oldLongitude, oldHeight, oldVn, oldVe, oldVd,
1190 vn, ve, vd, result);
1191 }
1192
1193 /**
1194 * Estimates curvilinear position by integrating the velocity.
1195 *
1196 * @param timeInterval time interval between epochs expressed in seconds (s).
1197 * @param oldLatitude previous latitude expressed in radians (rad).
1198 * @param oldLongitude previous longitude expressed in radians (rad).
1199 * @param oldHeight previous height expressed in meters (m).
1200 * @param oldVn previous velocity of body with respect the Earth, resolved about
1201 * north-axis.
1202 * @param oldVe previous velocity of body with respect the Earth, resolved about
1203 * east-axis.
1204 * @param oldVd previous velocity of body with respect the Earth, resolved about
1205 * down-axis.
1206 * @param vn current velocity of body with respect the Earth, resolved about
1207 * north-axis.
1208 * @param ve current velocity of body with respect the Earth, resolved about
1209 * east-axis.
1210 * @param vd current velocity of body with respect the Earth, resolved about
1211 * down-axis.
1212 * @param result instance where updated curvilinear position will be stored.
1213 * @throws IllegalArgumentException if provided time interval is negative.
1214 */
1215 public static void estimatePosition(
1216 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
1217 final Speed oldVn, final Speed oldVe, final Speed oldVd, final Speed vn, final Speed ve, final Speed vd,
1218 final NEDPosition result) {
1219 estimatePosition(timeInterval, oldLatitude, oldLongitude, oldHeight,
1220 convertSpeedToDouble(oldVn), convertSpeedToDouble(oldVe), convertSpeedToDouble(oldVd),
1221 convertSpeedToDouble(vn), convertSpeedToDouble(ve), convertSpeedToDouble(vd), result);
1222 }
1223
1224 /**
1225 * Estimates curvilinear position by integrating the velocity.
1226 *
1227 * @param timeInterval time interval between epochs.
1228 * @param oldLatitude previous latitude expressed in radians (rad).
1229 * @param oldLongitude previous longitude expressed in radians (rad).
1230 * @param oldHeight previous height expressed in meters (m).
1231 * @param oldVn previous velocity of body with respect the Earth, resolved about
1232 * north-axis.
1233 * @param oldVe previous velocity of body with respect the Earth, resolved about
1234 * east-axis.
1235 * @param oldVd previous velocity of body with respect the Earth, resolved about
1236 * down-axis.
1237 * @param vn current velocity of body with respect the Earth, resolved about
1238 * north-axis.
1239 * @param ve current velocity of body with respect the Earth, resolved about
1240 * east-axis.
1241 * @param vd current velocity of body with respect the Earth, resolved about
1242 * down-axis.
1243 * @param result instance where updated curvilinear position will be stored.
1244 * @throws IllegalArgumentException if provided time interval is negative.
1245 */
1246 public static void estimatePosition(
1247 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
1248 final Speed oldVn, final Speed oldVe, final Speed oldVd, final Speed vn, final Speed ve, final Speed vd,
1249 final NEDPosition result) {
1250 estimatePosition(convertTimeToDouble(timeInterval), oldLatitude, oldLongitude, oldHeight, oldVn, oldVe, oldVd,
1251 vn, ve, vd, result);
1252 }
1253
1254 /**
1255 * Estimates curvilinear position by integrating the velocity.
1256 *
1257 * @param timeInterval time interval between epochs expressed in seconds (s).
1258 * @param oldPosition previous body position with respect the Earth, resolved about
1259 * north, east and down axes.
1260 * @param oldVn previous velocity of body with respect the Earth, resolved about
1261 * north-axis and expressed in meters per second (m/s).
1262 * @param oldVe previous velocity of body with respect the Earth, resolved about
1263 * east-axis and expressed in meters per second (m/s).
1264 * @param oldVd previous velocity of body with respect the Earth, resolved about
1265 * down-axis and expressed in meters per second (m/s).
1266 * @param vn current velocity of body with respect the Earth, resolved about
1267 * north-axis and expressed in meters per second (m/s).
1268 * @param ve current velocity of body with respect the Earth, resolved about
1269 * east-axis and expressed in meters per second (m/s).
1270 * @param vd current velocity of body with respect the Earth, resolved about
1271 * down-axis and expressed in meters per second (m/s).
1272 * @param result instance where updated curvilinear position will be stored.
1273 * @throws IllegalArgumentException if provided time interval is negative.
1274 */
1275 public static void estimatePosition(
1276 final double timeInterval, final NEDPosition oldPosition,
1277 final double oldVn, final double oldVe, final double oldVd,
1278 final double vn, final double ve, final double vd, final NEDPosition result) {
1279 estimatePosition(timeInterval, oldPosition.getLatitude(), oldPosition.getLongitude(), oldPosition.getHeight(),
1280 oldVn, oldVe, oldVd, vn, ve, vd, result);
1281 }
1282
1283 /**
1284 * Estimates curvilinear position by integrating the velocity.
1285 *
1286 * @param timeInterval time interval between epochs.
1287 * @param oldPosition previous body position with respect the Earth, resolved about
1288 * north, east and down axes.
1289 * @param oldVn previous velocity of body with respect the Earth, resolved about
1290 * north-axis and expressed in meters per second (m/s).
1291 * @param oldVe previous velocity of body with respect the Earth, resolved about
1292 * east-axis and expressed in meters per second (m/s).
1293 * @param oldVd previous velocity of body with respect the Earth, resolved about
1294 * down-axis and expressed in meters per second (m/s).
1295 * @param vn current velocity of body with respect the Earth, resolved about
1296 * north-axis and expressed in meters per second (m/s).
1297 * @param ve current velocity of body with respect the Earth, resolved about
1298 * east-axis and expressed in meters per second (m/s).
1299 * @param vd current velocity of body with respect the Earth, resolved about
1300 * down-axis and expressed in meters per second (m/s).
1301 * @param result instance where updated curvilinear position will be stored.
1302 * @throws IllegalArgumentException if provided time interval is negative.
1303 */
1304 public static void estimatePosition(
1305 final Time timeInterval, final NEDPosition oldPosition,
1306 final double oldVn, final double oldVe, final double oldVd,
1307 final double vn, final double ve, final double vd, final NEDPosition result) {
1308 estimatePosition(convertTimeToDouble(timeInterval), oldPosition, oldVn, oldVe, oldVd, vn, ve, vd, result);
1309 }
1310
1311 /**
1312 * Estimates curvilinear position by integrating the velocity.
1313 *
1314 * @param timeInterval time interval between epochs expressed in seconds (s).
1315 * @param oldPosition previous body position with respect the Earth, resolved about
1316 * north, east and down axes.
1317 * @param oldVn previous velocity of body with respect the Earth, resolved about
1318 * north-axis.
1319 * @param oldVe previous velocity of body with respect the Earth, resolved about
1320 * east-axis.
1321 * @param oldVd previous velocity of body with respect the Earth, resolved about
1322 * down-axis.
1323 * @param vn current velocity of body with respect the Earth, resolved about
1324 * north-axis.
1325 * @param ve current velocity of body with respect the Earth, resolved about
1326 * east-axis.
1327 * @param vd current velocity of body with respect the Earth, resolved about
1328 * down-axis.
1329 * @param result instance where updated curvilinear position will be stored.
1330 * @throws IllegalArgumentException if provided time interval is negative.
1331 */
1332 public static void estimatePosition(
1333 final double timeInterval, final NEDPosition oldPosition,
1334 final Speed oldVn, final Speed oldVe, final Speed oldVd, final Speed vn, final Speed ve, final Speed vd,
1335 final NEDPosition result) {
1336 estimatePosition(timeInterval, oldPosition,
1337 convertSpeedToDouble(oldVn), convertSpeedToDouble(oldVe), convertSpeedToDouble(oldVd),
1338 convertSpeedToDouble(vn), convertSpeedToDouble(ve), convertSpeedToDouble(vd), result);
1339 }
1340
1341 /**
1342 * Estimates curvilinear position by integrating the velocity.
1343 *
1344 * @param timeInterval time interval between epochs.
1345 * @param oldPosition previous body position with respect the Earth, resolved about
1346 * north, east and down axes.
1347 * @param oldVn previous velocity of body with respect the Earth, resolved about
1348 * north-axis.
1349 * @param oldVe previous velocity of body with respect the Earth, resolved about
1350 * east-axis.
1351 * @param oldVd previous velocity of body with respect the Earth, resolved about
1352 * down-axis.
1353 * @param vn current velocity of body with respect the Earth, resolved about
1354 * north-axis.
1355 * @param ve current velocity of body with respect the Earth, resolved about
1356 * east-axis.
1357 * @param vd current velocity of body with respect the Earth, resolved about
1358 * down-axis.
1359 * @param result instance where updated curvilinear position will be stored.
1360 * @throws IllegalArgumentException if provided time interval is negative.
1361 */
1362 public static void estimatePosition(
1363 final Time timeInterval, final NEDPosition oldPosition,
1364 final Speed oldVn, final Speed oldVe, final Speed oldVd,
1365 final Speed vn, final Speed ve, final Speed vd, final NEDPosition result) {
1366 estimatePosition(convertTimeToDouble(timeInterval), oldPosition, oldVn, oldVe, oldVd, vn, ve, vd, result);
1367 }
1368
1369 /**
1370 * Estimates curvilinear position by integrating the velocity.
1371 *
1372 * @param timeInterval time interval between epochs expressed in seconds (s).
1373 * @param oldLatitude previous latitude expressed in radians (rad).
1374 * @param oldLongitude previous longitude expressed in radians (rad).
1375 * @param oldHeight previous height expressed in meters (m).
1376 * @param oldVelocity previous velocity of body with respect the Earth, resolved
1377 * about north, east and down axes and expressed in meters per
1378 * second (m/s).
1379 * @param velocity current velocity of body with respect the Earth, resolved
1380 * about north, east and down axes and expressed in meters per
1381 * second (m/s).
1382 * @param result instance where updated curvilinear position will be stored.
1383 * @throws IllegalArgumentException if provided time interval is negative.
1384 */
1385 public static void estimatePosition(
1386 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
1387 final NEDVelocity oldVelocity, final NEDVelocity velocity, final NEDPosition result) {
1388 estimatePosition(timeInterval, oldLatitude, oldLongitude, oldHeight,
1389 oldVelocity.getVn(), oldVelocity.getVe(), oldVelocity.getVd(),
1390 velocity.getVn(), velocity.getVe(), velocity.getVd(), result);
1391 }
1392
1393 /**
1394 * Estimates curvilinear position by integrating the velocity.
1395 *
1396 * @param timeInterval time interval between epochs.
1397 * @param oldLatitude previous latitude expressed in radians (rad).
1398 * @param oldLongitude previous longitude expressed in radians (rad).
1399 * @param oldHeight previous height expressed in meters (m).
1400 * @param oldVelocity previous velocity of body with respect the Earth, resolved
1401 * about north, east and down axes and expressed in meters per
1402 * second (m/s).
1403 * @param velocity current velocity of body with respect the Earth, resolved
1404 * about north, east and down axes and expressed in meters per
1405 * second (m/s).
1406 * @param result instance where updated curvilinear position will be stored.
1407 * @throws IllegalArgumentException if provided time interval is negative.
1408 */
1409 public static void estimatePosition(
1410 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
1411 final NEDVelocity oldVelocity, final NEDVelocity velocity, final NEDPosition result) {
1412 estimatePosition(convertTimeToDouble(timeInterval), oldLatitude, oldLongitude, oldHeight, oldVelocity, velocity,
1413 result);
1414 }
1415
1416 /**
1417 * Estimates velocity from curvilinear position changes and taking into account previous
1418 * velocity respect NED frame.
1419 *
1420 * @param timeInterval time interval between epochs expressed in seconds (s).
1421 * @param oldLatitude previous latitude.
1422 * @param oldLongitude previous longitude.
1423 * @param oldHeight previous height.
1424 * @param oldVelocity previous velocity of body with respect the Earth, resolved
1425 * about north, east and down axes and expressed in meters per
1426 * second (m/s).
1427 * @param velocity current velocity of body with respect the Earth, resolved
1428 * about north, east and down axes and expressed in meters per
1429 * second (m/s).
1430 * @param result instance where updated curvilinear position will be stored.
1431 * @throws IllegalArgumentException if provided time interval is negative.
1432 */
1433 public static void estimatePosition(
1434 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
1435 final NEDVelocity oldVelocity, final NEDVelocity velocity, final NEDPosition result) {
1436 estimatePosition(timeInterval, convertAngleToDouble(oldLatitude), convertAngleToDouble(oldLongitude),
1437 convertDistanceToDouble(oldHeight), oldVelocity, velocity, result);
1438 }
1439
1440 /**
1441 * Estimates velocity from curvilinear position changes and taking into account previous
1442 * velocity respect NED frame.
1443 *
1444 * @param timeInterval time interval between epochs.
1445 * @param oldLatitude previous latitude.
1446 * @param oldLongitude previous longitude.
1447 * @param oldHeight previous height.
1448 * @param oldVelocity previous velocity of body with respect the Earth, resolved
1449 * about north, east and down axes and expressed in meters per
1450 * second (m/s).
1451 * @param velocity current velocity of body with respect the Earth, resolved
1452 * about north, east and down axes and expressed in meters per
1453 * second (m/s).
1454 * @param result instance where updated curvilinear position will be stored.
1455 * @throws IllegalArgumentException if provided time interval is negative.
1456 */
1457 public static void estimatePosition(
1458 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
1459 final NEDVelocity oldVelocity, final NEDVelocity velocity, final NEDPosition result) {
1460 estimatePosition(convertTimeToDouble(timeInterval), oldLatitude, oldLongitude, oldHeight, oldVelocity, velocity,
1461 result);
1462 }
1463
1464 /**
1465 * Estimates curvilinear position by integrating the velocity.
1466 *
1467 * @param timeInterval time interval between epochs expressed in seconds (s).
1468 * @param oldPosition previous body position with respect the Earth, resolved about
1469 * north, east and down axes.
1470 * @param oldVelocity previous velocity of body with respect the Earth, resolved
1471 * about north, east and down axes and expressed in meters per
1472 * second (m/s).
1473 * @param velocity current velocity of body with respect the Earth, resolved
1474 * about north, east and down axes and expressed in meters per
1475 * second (m/s).
1476 * @param result instance where updated curvilinear position will be stored.
1477 * @throws IllegalArgumentException if provided time interval is negative.
1478 */
1479 public static void estimatePosition(
1480 final double timeInterval, final NEDPosition oldPosition, final NEDVelocity oldVelocity,
1481 final NEDVelocity velocity, final NEDPosition result) {
1482 estimatePosition(timeInterval, oldPosition.getLatitude(), oldPosition.getLongitude(), oldPosition.getHeight(),
1483 oldVelocity, velocity, result);
1484 }
1485
1486 /**
1487 * Estimates curvilinear position by integrating the velocity.
1488 *
1489 * @param timeInterval time interval between epochs.
1490 * @param oldPosition previous body position with respect the Earth, resolved about
1491 * north, east and down axes.
1492 * @param oldVelocity previous velocity of body with respect the Earth, resolved
1493 * about north, east and down axes and expressed in meters per
1494 * second (m/s).
1495 * @param velocity current velocity of body with respect the Earth, resolved
1496 * about north, east and down axes and expressed in meters per
1497 * second (m/s).
1498 * @param result instance where updated curvilinear position will be stored.
1499 * @throws IllegalArgumentException if provided time interval is negative.
1500 */
1501 public static void estimatePosition(
1502 final Time timeInterval, final NEDPosition oldPosition, final NEDVelocity oldVelocity,
1503 final NEDVelocity velocity, final NEDPosition result) {
1504 estimatePosition(convertTimeToDouble(timeInterval), oldPosition, oldVelocity, velocity, result);
1505 }
1506
1507 /**
1508 * Estimates curvilinear position by integrating the velocity.
1509 *
1510 * @param timeInterval time interval between epochs expressed in seconds (s).
1511 * @param oldLatitude previous latitude expressed in radians (rad).
1512 * @param oldLongitude previous longitude expressed in radians (rad).
1513 * @param oldHeight previous height expressed in meters (m).
1514 * @param oldVn previous velocity of body with respect the Earth, resolved about
1515 * north-axis and expressed in meters per second (m/s).
1516 * @param oldVe previous velocity of body with respect the Earth, resolved about
1517 * east-axis and expressed in meters per second (m/s).
1518 * @param oldVd previous velocity of body with respect the Earth, resolved about
1519 * down-axis and expressed in meters per second (m/s).
1520 * @param vn current velocity of body with respect the Earth, resolved about
1521 * north-axis and expressed in meters per second (m/s).
1522 * @param ve current velocity of body with respect the Earth, resolved about
1523 * east-axis and expressed in meters per second (m/s).
1524 * @param vd current velocity of body with respect the Earth, resolved about
1525 * down-axis and expressed in meters per second (m/s).
1526 * @return new updated curvilinear position.
1527 * @throws IllegalArgumentException if provided time interval is negative.
1528 */
1529 public static NEDPosition estimatePositionAndReturnNew(
1530 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
1531 final double oldVn, final double oldVe, final double oldVd,
1532 final double vn, final double ve, final double vd) {
1533 final var result = new NEDPosition();
1534 estimatePosition(timeInterval, oldLatitude, oldLongitude, oldHeight, oldVn, oldVe, oldVd, vn, ve, vd, result);
1535 return result;
1536 }
1537
1538 /**
1539 * Estimates curvilinear position by integrating the velocity.
1540 *
1541 * @param timeInterval time interval between epochs.
1542 * @param oldLatitude previous latitude expressed in radians (rad).
1543 * @param oldLongitude previous longitude expressed in radians (rad).
1544 * @param oldHeight previous height expressed in meters (m).
1545 * @param oldVn previous velocity of body with respect the Earth, resolved about
1546 * north-axis and expressed in meters per second (m/s).
1547 * @param oldVe previous velocity of body with respect the Earth, resolved about
1548 * east-axis and expressed in meters per second (m/s).
1549 * @param oldVd previous velocity of body with respect the Earth, resolved about
1550 * down-axis and expressed in meters per second (m/s).
1551 * @param vn current velocity of body with respect the Earth, resolved about
1552 * north-axis and expressed in meters per second (m/s).
1553 * @param ve current velocity of body with respect the Earth, resolved about
1554 * east-axis and expressed in meters per second (m/s).
1555 * @param vd current velocity of body with respect the Earth, resolved about
1556 * down-axis and expressed in meters per second (m/s).
1557 * @return new updated curvilinear position.
1558 * @throws IllegalArgumentException if provided time interval is negative.
1559 */
1560 public static NEDPosition estimatePositionAndReturnNew(
1561 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
1562 final double oldVn, final double oldVe, final double oldVd,
1563 final double vn, final double ve, final double vd) {
1564 final var result = new NEDPosition();
1565 estimatePosition(timeInterval, oldLatitude, oldLongitude, oldHeight, oldVn, oldVe, oldVd, vn, ve, vd, result);
1566 return result;
1567 }
1568
1569 /**
1570 * Estimates curvilinear position by integrating the velocity.
1571 *
1572 * @param timeInterval time interval between epochs expressed in seconds (s).
1573 * @param oldLatitude previous latitude.
1574 * @param oldLongitude previous longitude.
1575 * @param oldHeight previous height.
1576 * @param oldVn previous velocity of body with respect the Earth, resolved about
1577 * north-axis.
1578 * @param oldVe previous velocity of body with respect the Earth, resolved about
1579 * east-axis.
1580 * @param oldVd previous velocity of body with respect the Earth, resolved about
1581 * down-axis.
1582 * @param vn current velocity of body with respect the Earth, resolved about
1583 * north-axis.
1584 * @param ve current velocity of body with respect the Earth, resolved about
1585 * east-axis.
1586 * @param vd current velocity of body with respect the Earth, resolved about
1587 * down-axis.
1588 * @return new updated curvilinear position.
1589 * @throws IllegalArgumentException if provided time interval is negative.
1590 */
1591 public static NEDPosition estimatePositionAndReturnNew(
1592 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
1593 final Speed oldVn, final Speed oldVe, final Speed oldVd, final Speed vn, final Speed ve, final Speed vd) {
1594 final var result = new NEDPosition();
1595 estimatePosition(timeInterval, oldLatitude, oldLongitude, oldHeight, oldVn, oldVe, oldVd, vn, ve, vd, result);
1596 return result;
1597 }
1598
1599 /**
1600 * Estimates curvilinear position by integrating the velocity.
1601 *
1602 * @param timeInterval time interval between epochs.
1603 * @param oldLatitude previous latitude.
1604 * @param oldLongitude previous longitude.
1605 * @param oldHeight previous height.
1606 * @param oldVn previous velocity of body with respect the Earth, resolved about
1607 * north-axis.
1608 * @param oldVe previous velocity of body with respect the Earth, resolved about
1609 * east-axis.
1610 * @param oldVd previous velocity of body with respect the Earth, resolved about
1611 * down-axis.
1612 * @param vn current velocity of body with respect the Earth, resolved about
1613 * north-axis.
1614 * @param ve current velocity of body with respect the Earth, resolved about
1615 * east-axis.
1616 * @param vd current velocity of body with respect the Earth, resolved about
1617 * down-axis.
1618 * @return new updated curvilinear position.
1619 * @throws IllegalArgumentException if provided time interval is negative.
1620 */
1621 public static NEDPosition estimatePositionAndReturnNew(
1622 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
1623 final Speed oldVn, final Speed oldVe, final Speed oldVd, final Speed vn, final Speed ve, final Speed vd) {
1624 final var result = new NEDPosition();
1625 estimatePosition(timeInterval, oldLatitude, oldLongitude, oldHeight, oldVn, oldVe, oldVd, vn, ve, vd, result);
1626 return result;
1627 }
1628
1629 /**
1630 * Estimates curvilinear position by integrating the velocity.
1631 *
1632 * @param timeInterval time interval between epochs expressed in seconds (s).
1633 * @param oldLatitude previous latitude.
1634 * @param oldLongitude previous longitude.
1635 * @param oldHeight previous height.
1636 * @param oldVn previous velocity of body with respect the Earth, resolved about
1637 * north-axis and expressed in meters per second (m/s).
1638 * @param oldVe previous velocity of body with respect the Earth, resolved about
1639 * east-axis and expressed in meters per second (m/s).
1640 * @param oldVd previous velocity of body with respect the Earth, resolved about
1641 * down-axis and expressed in meters per second (m/s).
1642 * @param vn current velocity of body with respect the Earth, resolved about
1643 * north-axis and expressed in meters per second (m/s).
1644 * @param ve current velocity of body with respect the Earth, resolved about
1645 * east-axis and expressed in meters per second (m/s).
1646 * @param vd current velocity of body with respect the Earth, resolved about
1647 * down-axis and expressed in meters per second (m/s).
1648 * @return new updated curvilinear position.
1649 * @throws IllegalArgumentException if provided time interval is negative.
1650 */
1651 public static NEDPosition estimatePositionAndReturnNew(
1652 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
1653 final double oldVn, final double oldVe, final double oldVd,
1654 final double vn, final double ve, final double vd) {
1655 final var result = new NEDPosition();
1656 estimatePosition(timeInterval, oldLatitude, oldLongitude, oldHeight, oldVn, oldVe, oldVd, vn, ve, vd, result);
1657 return result;
1658 }
1659
1660 /**
1661 * Estimates curvilinear position by integrating the velocity.
1662 *
1663 * @param timeInterval time interval between epochs.
1664 * @param oldLatitude previous latitude.
1665 * @param oldLongitude previous longitude.
1666 * @param oldHeight previous height.
1667 * @param oldVn previous velocity of body with respect the Earth, resolved about
1668 * north-axis and expressed in meters per second (m/s).
1669 * @param oldVe previous velocity of body with respect the Earth, resolved about
1670 * east-axis and expressed in meters per second (m/s).
1671 * @param oldVd previous velocity of body with respect the Earth, resolved about
1672 * down-axis and expressed in meters per second (m/s).
1673 * @param vn current velocity of body with respect the Earth, resolved about
1674 * north-axis and expressed in meters per second (m/s).
1675 * @param ve current velocity of body with respect the Earth, resolved about
1676 * east-axis and expressed in meters per second (m/s).
1677 * @param vd current velocity of body with respect the Earth, resolved about
1678 * down-axis and expressed in meters per second (m/s).
1679 * @return new updated curvilinear position.
1680 * @throws IllegalArgumentException if provided time interval is negative.
1681 */
1682 public static NEDPosition estimatePositionAndReturnNew(
1683 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
1684 final double oldVn, final double oldVe, final double oldVd,
1685 final double vn, final double ve, final double vd) {
1686 final var result = new NEDPosition();
1687 estimatePosition(timeInterval, oldLatitude, oldLongitude, oldHeight, oldVn, oldVe, oldVd, vn, ve, vd, result);
1688 return result;
1689 }
1690
1691 /**
1692 * Estimates curvilinear position by integrating the velocity.
1693 *
1694 * @param timeInterval time interval between epochs expressed in seconds (s).
1695 * @param oldLatitude previous latitude expressed in radians (rad).
1696 * @param oldLongitude previous longitude expressed in radians (rad).
1697 * @param oldHeight previous height expressed in meters (m).
1698 * @param oldVn previous velocity of body with respect the Earth, resolved about
1699 * north-axis.
1700 * @param oldVe previous velocity of body with respect the Earth, resolved about
1701 * east-axis.
1702 * @param oldVd previous velocity of body with respect the Earth, resolved about
1703 * down-axis.
1704 * @param vn current velocity of body with respect the Earth, resolved about
1705 * north-axis.
1706 * @param ve current velocity of body with respect the Earth, resolved about
1707 * east-axis.
1708 * @param vd current velocity of body with respect the Earth, resolved about
1709 * down-axis.
1710 * @return new updated curvilinear position.
1711 * @throws IllegalArgumentException if provided time interval is negative.
1712 */
1713 public static NEDPosition estimatePositionAndReturnNew(
1714 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
1715 final Speed oldVn, final Speed oldVe, final Speed oldVd, final Speed vn, final Speed ve, final Speed vd) {
1716 final var result = new NEDPosition();
1717 estimatePosition(timeInterval, oldLatitude, oldLongitude, oldHeight, oldVn, oldVe, oldVd, vn, ve, vd, result);
1718 return result;
1719 }
1720
1721 /**
1722 * Estimates curvilinear position by integrating the velocity.
1723 *
1724 * @param timeInterval time interval between epochs.
1725 * @param oldLatitude previous latitude expressed in radians (rad).
1726 * @param oldLongitude previous longitude expressed in radians (rad).
1727 * @param oldHeight previous height expressed in meters (m).
1728 * @param oldVn previous velocity of body with respect the Earth, resolved about
1729 * north-axis.
1730 * @param oldVe previous velocity of body with respect the Earth, resolved about
1731 * east-axis.
1732 * @param oldVd previous velocity of body with respect the Earth, resolved about
1733 * down-axis.
1734 * @param vn current velocity of body with respect the Earth, resolved about
1735 * north-axis.
1736 * @param ve current velocity of body with respect the Earth, resolved about
1737 * east-axis.
1738 * @param vd current velocity of body with respect the Earth, resolved about
1739 * down-axis.
1740 * @return new updated curvilinear position.
1741 * @throws IllegalArgumentException if provided time interval is negative.
1742 */
1743 public static NEDPosition estimatePositionAndReturnNew(
1744 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
1745 final Speed oldVn, final Speed oldVe, final Speed oldVd, final Speed vn, final Speed ve, final Speed vd) {
1746 final var result = new NEDPosition();
1747 estimatePosition(timeInterval, oldLatitude, oldLongitude, oldHeight, oldVn, oldVe, oldVd, vn, ve, vd, result);
1748 return result;
1749 }
1750
1751 /**
1752 * Estimates curvilinear position by integrating the velocity.
1753 *
1754 * @param timeInterval time interval between epochs expressed in seconds (s).
1755 * @param oldPosition previous body position with respect the Earth, resolved about
1756 * north, east and down axes.
1757 * @param oldVn previous velocity of body with respect the Earth, resolved about
1758 * north-axis and expressed in meters per second (m/s).
1759 * @param oldVe previous velocity of body with respect the Earth, resolved about
1760 * east-axis and expressed in meters per second (m/s).
1761 * @param oldVd previous velocity of body with respect the Earth, resolved about
1762 * down-axis and expressed in meters per second (m/s).
1763 * @param vn current velocity of body with respect the Earth, resolved about
1764 * north-axis and expressed in meters per second (m/s).
1765 * @param ve current velocity of body with respect the Earth, resolved about
1766 * east-axis and expressed in meters per second (m/s).
1767 * @param vd current velocity of body with respect the Earth, resolved about
1768 * down-axis and expressed in meters per second (m/s).
1769 * @return new updated curvilinear position.
1770 * @throws IllegalArgumentException if provided time interval is negative.
1771 */
1772 public static NEDPosition estimatePositionAndReturnNew(
1773 final double timeInterval, final NEDPosition oldPosition,
1774 final double oldVn, final double oldVe, final double oldVd,
1775 final double vn, final double ve, final double vd) {
1776 final var result = new NEDPosition();
1777 estimatePosition(timeInterval, oldPosition, oldVn, oldVe, oldVd, vn, ve, vd, result);
1778 return result;
1779 }
1780
1781 /**
1782 * Estimates curvilinear position by integrating the velocity.
1783 *
1784 * @param timeInterval time interval between epochs.
1785 * @param oldPosition previous body position with respect the Earth, resolved about
1786 * north, east and down axes.
1787 * @param oldVn previous velocity of body with respect the Earth, resolved about
1788 * north-axis and expressed in meters per second (m/s).
1789 * @param oldVe previous velocity of body with respect the Earth, resolved about
1790 * east-axis and expressed in meters per second (m/s).
1791 * @param oldVd previous velocity of body with respect the Earth, resolved about
1792 * down-axis and expressed in meters per second (m/s).
1793 * @param vn current velocity of body with respect the Earth, resolved about
1794 * north-axis and expressed in meters per second (m/s).
1795 * @param ve current velocity of body with respect the Earth, resolved about
1796 * east-axis and expressed in meters per second (m/s).
1797 * @param vd current velocity of body with respect the Earth, resolved about
1798 * down-axis and expressed in meters per second (m/s).
1799 * @return new updated curvilinear position.
1800 * @throws IllegalArgumentException if provided time interval is negative.
1801 */
1802 public static NEDPosition estimatePositionAndReturnNew(
1803 final Time timeInterval, final NEDPosition oldPosition,
1804 final double oldVn, final double oldVe, final double oldVd,
1805 final double vn, final double ve, final double vd) {
1806 final var result = new NEDPosition();
1807 estimatePosition(timeInterval, oldPosition, oldVn, oldVe, oldVd, vn, ve, vd, result);
1808 return result;
1809 }
1810
1811 /**
1812 * Estimates curvilinear position by integrating the velocity.
1813 *
1814 * @param timeInterval time interval between epochs expressed in seconds (s).
1815 * @param oldPosition previous body position with respect the Earth, resolved about
1816 * north, east and down axes.
1817 * @param oldVn previous velocity of body with respect the Earth, resolved about
1818 * north-axis.
1819 * @param oldVe previous velocity of body with respect the Earth, resolved about
1820 * east-axis.
1821 * @param oldVd previous velocity of body with respect the Earth, resolved about
1822 * down-axis.
1823 * @param vn current velocity of body with respect the Earth, resolved about
1824 * north-axis.
1825 * @param ve current velocity of body with respect the Earth, resolved about
1826 * east-axis.
1827 * @param vd current velocity of body with respect the Earth, resolved about
1828 * down-axis.
1829 * @return new updated curvilinear position.
1830 * @throws IllegalArgumentException if provided time interval is negative.
1831 */
1832 public static NEDPosition estimatePositionAndReturnNew(
1833 final double timeInterval, final NEDPosition oldPosition,
1834 final Speed oldVn, final Speed oldVe, final Speed oldVd, final Speed vn, final Speed ve, final Speed vd) {
1835 final var result = new NEDPosition();
1836 estimatePosition(timeInterval, oldPosition, oldVn, oldVe, oldVd, vn, ve, vd, result);
1837 return result;
1838 }
1839
1840 /**
1841 * Estimates curvilinear position by integrating the velocity.
1842 *
1843 * @param timeInterval time interval between epochs.
1844 * @param oldPosition previous body position with respect the Earth, resolved about
1845 * north, east and down axes.
1846 * @param oldVn previous velocity of body with respect the Earth, resolved about
1847 * north-axis.
1848 * @param oldVe previous velocity of body with respect the Earth, resolved about
1849 * east-axis.
1850 * @param oldVd previous velocity of body with respect the Earth, resolved about
1851 * down-axis.
1852 * @param vn current velocity of body with respect the Earth, resolved about
1853 * north-axis.
1854 * @param ve current velocity of body with respect the Earth, resolved about
1855 * east-axis.
1856 * @param vd current velocity of body with respect the Earth, resolved about
1857 * down-axis.
1858 * @return new updated curvilinear position.
1859 * @throws IllegalArgumentException if provided time interval is negative.
1860 */
1861 public static NEDPosition estimatePositionAndReturnNew(
1862 final Time timeInterval, final NEDPosition oldPosition,
1863 final Speed oldVn, final Speed oldVe, final Speed oldVd, final Speed vn, final Speed ve, final Speed vd) {
1864 final var result = new NEDPosition();
1865 estimatePosition(timeInterval, oldPosition, oldVn, oldVe, oldVd, vn, ve, vd, result);
1866 return result;
1867 }
1868
1869 /**
1870 * Estimates curvilinear position by integrating the velocity.
1871 *
1872 * @param timeInterval time interval between epochs expressed in seconds (s).
1873 * @param oldLatitude previous latitude expressed in radians (rad).
1874 * @param oldLongitude previous longitude expressed in radians (rad).
1875 * @param oldHeight previous height expressed in meters (m).
1876 * @param oldVelocity previous velocity of body with respect the Earth, resolved
1877 * about north, east and down axes and expressed in meters per
1878 * second (m/s).
1879 * @param velocity current velocity of body with respect the Earth, resolved
1880 * about north, east and down axes and expressed in meters per
1881 * second (m/s).
1882 * @return new updated curvilinear position.
1883 * @throws IllegalArgumentException if provided time interval is negative.
1884 */
1885 public static NEDPosition estimatePositionAndReturnNew(
1886 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
1887 final NEDVelocity oldVelocity, final NEDVelocity velocity) {
1888 final var result = new NEDPosition();
1889 estimatePosition(timeInterval, oldLatitude, oldLongitude, oldHeight, oldVelocity, velocity, result);
1890 return result;
1891 }
1892
1893 /**
1894 * Estimates curvilinear position by integrating the velocity.
1895 *
1896 * @param timeInterval time interval between epochs.
1897 * @param oldLatitude previous latitude expressed in radians (rad).
1898 * @param oldLongitude previous longitude expressed in radians (rad).
1899 * @param oldHeight previous height expressed in meters (m).
1900 * @param oldVelocity previous velocity of body with respect the Earth, resolved
1901 * about north, east and down axes and expressed in meters per
1902 * second (m/s).
1903 * @param velocity current velocity of body with respect the Earth, resolved
1904 * about north, east and down axes and expressed in meters per
1905 * second (m/s).
1906 * @return new updated curvilinear position.
1907 * @throws IllegalArgumentException if provided time interval is negative.
1908 */
1909 public static NEDPosition estimatePositionAndReturnNew(
1910 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
1911 final NEDVelocity oldVelocity, final NEDVelocity velocity) {
1912 final var result = new NEDPosition();
1913 estimatePosition(timeInterval, oldLatitude, oldLongitude, oldHeight, oldVelocity, velocity, result);
1914 return result;
1915 }
1916
1917 /**
1918 * Estimates velocity from curvilinear position changes and taking into account previous
1919 * velocity respect NED frame.
1920 *
1921 * @param timeInterval time interval between epochs expressed in seconds (s).
1922 * @param oldLatitude previous latitude.
1923 * @param oldLongitude previous longitude.
1924 * @param oldHeight previous height.
1925 * @param oldVelocity previous velocity of body with respect the Earth, resolved
1926 * about north, east and down axes and expressed in meters per
1927 * second (m/s).
1928 * @param velocity current velocity of body with respect the Earth, resolved
1929 * about north, east and down axes and expressed in meters per
1930 * second (m/s).
1931 * @return new updated curvilinear position.
1932 * @throws IllegalArgumentException if provided time interval is negative.
1933 */
1934 public static NEDPosition estimatePositionAndReturnNew(
1935 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
1936 final NEDVelocity oldVelocity, final NEDVelocity velocity) {
1937 final var result = new NEDPosition();
1938 estimatePosition(timeInterval, oldLatitude, oldLongitude, oldHeight, oldVelocity, velocity, result);
1939 return result;
1940 }
1941
1942 /**
1943 * Estimates velocity from curvilinear position changes and taking into account previous
1944 * velocity respect NED frame.
1945 *
1946 * @param timeInterval time interval between epochs.
1947 * @param oldLatitude previous latitude.
1948 * @param oldLongitude previous longitude.
1949 * @param oldHeight previous height.
1950 * @param oldVelocity previous velocity of body with respect the Earth, resolved
1951 * about north, east and down axes and expressed in meters per
1952 * second (m/s).
1953 * @param velocity current velocity of body with respect the Earth, resolved
1954 * about north, east and down axes and expressed in meters per
1955 * second (m/s).
1956 * @return new updated curvilinear position.
1957 * @throws IllegalArgumentException if provided time interval is negative.
1958 */
1959 public static NEDPosition estimatePositionAndReturnNew(
1960 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
1961 final NEDVelocity oldVelocity, final NEDVelocity velocity) {
1962 final var result = new NEDPosition();
1963 estimatePosition(timeInterval, oldLatitude, oldLongitude, oldHeight, oldVelocity, velocity, result);
1964 return result;
1965 }
1966
1967 /**
1968 * Estimates curvilinear position by integrating the velocity.
1969 *
1970 * @param timeInterval time interval between epochs expressed in seconds (s).
1971 * @param oldPosition previous body position with respect the Earth, resolved about
1972 * north, east and down axes.
1973 * @param oldVelocity previous velocity of body with respect the Earth, resolved
1974 * about north, east and down axes and expressed in meters per
1975 * second (m/s).
1976 * @param velocity current velocity of body with respect the Earth, resolved
1977 * about north, east and down axes and expressed in meters per
1978 * second (m/s).
1979 * @return new updated curvilinear position.
1980 * @throws IllegalArgumentException if provided time interval is negative.
1981 */
1982 public static NEDPosition estimatePositionAndReturnNew(
1983 final double timeInterval, final NEDPosition oldPosition, final NEDVelocity oldVelocity,
1984 final NEDVelocity velocity) {
1985 final var result = new NEDPosition();
1986 estimatePosition(timeInterval, oldPosition, oldVelocity, velocity, result);
1987 return result;
1988 }
1989
1990 /**
1991 * Estimates curvilinear position by integrating the velocity.
1992 *
1993 * @param timeInterval time interval between epochs.
1994 * @param oldPosition previous body position with respect the Earth, resolved about
1995 * north, east and down axes.
1996 * @param oldVelocity previous velocity of body with respect the Earth, resolved
1997 * about north, east and down axes and expressed in meters per
1998 * second (m/s).
1999 * @param velocity current velocity of body with respect the Earth, resolved
2000 * about north, east and down axes and expressed in meters per
2001 * second (m/s).
2002 * @return new updated curvilinear position.
2003 * @throws IllegalArgumentException if provided time interval is negative.
2004 */
2005 public static NEDPosition estimatePositionAndReturnNew(
2006 final Time timeInterval, final NEDPosition oldPosition, final NEDVelocity oldVelocity,
2007 final NEDVelocity velocity) {
2008 final var result = new NEDPosition();
2009 estimatePosition(timeInterval, oldPosition, oldVelocity, velocity, result);
2010 return result;
2011 }
2012
2013 /**
2014 * Converts provided time instance to seconds (s).
2015 *
2016 * @param time time instance to be converted.
2017 * @return provided time value expressed in seconds.
2018 */
2019 private static double convertTimeToDouble(final Time time) {
2020 return TimeConverter.convert(time.getValue().doubleValue(), time.getUnit(), TimeUnit.SECOND);
2021 }
2022
2023 /**
2024 * Converts provided angle instance to radians (rad).
2025 *
2026 * @param angle angle instance to be converted.
2027 * @return provided angle value expressed in radians.
2028 */
2029 private static double convertAngleToDouble(final Angle angle) {
2030 return AngleConverter.convert(angle.getValue().doubleValue(), angle.getUnit(), AngleUnit.RADIANS);
2031 }
2032
2033 /**
2034 * Converts provided distance instance to meters (m).
2035 *
2036 * @param distance distance instance to be converted.
2037 * @return provided distance value expressed in meters.
2038 */
2039 private static double convertDistanceToDouble(final Distance distance) {
2040 return DistanceConverter.convert(distance.getValue().doubleValue(), distance.getUnit(), DistanceUnit.METER);
2041 }
2042
2043 /**
2044 * Converts provided speed instance to meters per second (m/s).
2045 *
2046 * @param speed speed instance to be converted.
2047 * @return provided speed value expressed in meters per second.
2048 */
2049 private static double convertSpeedToDouble(final Speed speed) {
2050 return SpeedConverter.convert(speed.getValue().doubleValue(), speed.getUnit(), SpeedUnit.METERS_PER_SECOND);
2051 }
2052 }