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.navigators;
17
18 import com.irurueta.algebra.AlgebraException;
19 import com.irurueta.algebra.Matrix;
20 import com.irurueta.algebra.Utils;
21 import com.irurueta.geometry.InvalidRotationMatrixException;
22 import com.irurueta.navigation.frames.CoordinateTransformation;
23 import com.irurueta.navigation.frames.FrameType;
24 import com.irurueta.navigation.frames.InvalidSourceAndDestinationFrameTypeException;
25 import com.irurueta.navigation.frames.NEDFrame;
26 import com.irurueta.navigation.frames.NEDPosition;
27 import com.irurueta.navigation.frames.NEDVelocity;
28 import com.irurueta.navigation.geodesic.Constants;
29 import com.irurueta.navigation.inertial.BodyKinematics;
30 import com.irurueta.navigation.inertial.estimators.NEDGravityEstimator;
31 import com.irurueta.navigation.inertial.estimators.RadiiOfCurvatureEstimator;
32 import com.irurueta.units.*;
33
34 /**
35 * Runs precision local-navigation-frame inertial navigation equations.
36 * NOTE: only the attitude update and specific force frame transformation phases are precise).
37 * This implementation is based on the equations defined in "Principles of GNSS, Inertial, and Multisensor
38 * Integrated Navigation Systems, Second Edition" and on the companion software available at:
39 * <a href="https://github.com/ymjdz/MATLAB-Codes/blob/master/Nav_equations_NED.m">
40 * https://github.com/ymjdz/MATLAB-Codes/blob/master/Nav_equations_NED.m
41 * </a>
42 */
43 public class NEDInertialNavigator {
44
45 /**
46 * Earth rotation rate expressed in radians per second (rad/s).
47 */
48 public static final double EARTH_ROTATION_RATE = Constants.EARTH_ROTATION_RATE;
49
50 /**
51 * Default threshold to consider a coordinate transformation matrix as a valid rotation.
52 */
53 public static final double DEFAULT_ACCURACY_THRESHOLD = CoordinateTransformation.DEFAULT_THRESHOLD;
54
55 /**
56 * Alpha threshold.
57 */
58 private static final double ALPHA_THRESHOLD = 1e-8;
59
60 /**
61 * Number of rows.
62 */
63 private static final int ROWS = 3;
64
65 /**
66 * Runs precision local-navigation-frame inertial navigation equations.
67 * NOTE: only the attitude update and specific force frame transformation
68 * phases are precise.
69 *
70 * @param timeInterval time interval between epochs expressed in seconds (s).
71 * @param oldLatitude previous latitude expressed in radians (rad).
72 * @param oldLongitude previous longitude expressed in radians (rad).
73 * @param oldHeight previous height expressed in meters (m).
74 * @param oldC previous body-to-NED coordinate transformation.
75 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
76 * resolved along NED-frame axes and expressed in meters per second (m/s).
77 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
78 * resolved along NED-frame axes and expressed in meters per second (m/s).
79 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
80 * resolved along NED-frame axes and expressed in meters per second (m/s).
81 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
82 * resolved along body-frame axes, averaged over time interval and
83 * expressed in meters per squared second (m/s^2).
84 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
85 * resolved along body-frame axes, averaged over time interval and
86 * expressed in meters per squared second (m/s^2).
87 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
88 * resolved along body-frame axes, averaged over time interval and
89 * expressed in meters per squared second (m/s^2).
90 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
91 * resolved along body-frame axes, averaged over time interval and
92 * expressed in radians per second (rad/s).
93 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
94 * resolved along body-frame axes, averaged over time interval and
95 * expressed in radians per second (rad/s).
96 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
97 * resolved along body-frame axes, averaged over time interval and
98 * expressed in radians per second (rad/s).
99 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
100 * @param result instance where new estimated NED frame containing new body position,
101 * velocity and coordinate transformation matrix will be stored.
102 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
103 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
104 * body-to-NED-frame coordinate transformation matrix are
105 * invalid.
106 */
107 public void navigate(
108 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
109 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
110 final double fx, final double fy, final double fz,
111 final double angularRateX, final double angularRateY, final double angularRateZ,
112 final double accuracyThreshold, final NEDFrame result)
113 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
114 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
115 oldVn, oldVe, oldVd, fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
116 }
117
118 /**
119 * Runs precision local-navigation-frame inertial navigation equations.
120 * NOTE: only the attitude update and specific force frame transformation
121 * phases are precise.
122 *
123 * @param timeInterval time interval between epochs expressed in seconds (s).
124 * @param oldLatitude previous latitude expressed in radians (rad).
125 * @param oldLongitude previous longitude expressed in radians (rad).
126 * @param oldHeight previous height expressed in meters (m).
127 * @param oldC previous body-to-NED coordinate transformation.
128 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
129 * resolved along NED-frame axes and expressed in meters per second (m/s).
130 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
131 * resolved along NED-frame axes and expressed in meters per second (m/s).
132 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
133 * resolved along NED-frame axes and expressed in meters per second (m/s).
134 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
135 * resolved along body-frame axes, averaged over time interval and
136 * expressed in meters per squared second (m/s^2).
137 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
138 * resolved along body-frame axes, averaged over time interval and
139 * expressed in meters per squared second (m/s^2).
140 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
141 * resolved along body-frame axes, averaged over time interval and
142 * expressed in meters per squared second (m/s^2).
143 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
144 * resolved along body-frame axes, averaged over time interval and
145 * expressed in radians per second (rad/s).
146 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
147 * resolved along body-frame axes, averaged over time interval and
148 * expressed in radians per second (rad/s).
149 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
150 * resolved along body-frame axes, averaged over time interval and
151 * expressed in radians per second (rad/s).
152 * @param result instance where new estimated NED frame containing new body position,
153 * velocity and coordinate transformation matrix will be stored.
154 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
155 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
156 * body-to-NED-frame coordinate transformation matrix are
157 * invalid.
158 */
159 public void navigate(
160 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
161 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
162 final double fx, final double fy, final double fz,
163 final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result)
164 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
165 navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
166 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
167 }
168
169 /**
170 * Runs precision local-navigation-frame inertial navigation equations.
171 * NOTE: only the attitude update and specific force frame transformation
172 * phases are precise.
173 *
174 * @param timeInterval time interval between epochs.
175 * @param oldLatitude previous latitude expressed in radians (rad).
176 * @param oldLongitude previous longitude expressed in radians (rad).
177 * @param oldHeight previous height expressed in meters (m).
178 * @param oldC previous body-to-NED coordinate transformation.
179 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
180 * resolved along NED-frame axes and expressed in meters per second (m/s).
181 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
182 * resolved along NED-frame axes and expressed in meters per second (m/s).
183 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
184 * resolved along NED-frame axes and expressed in meters per second (m/s).
185 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
186 * resolved along body-frame axes, averaged over time interval and
187 * expressed in meters per squared second (m/s^2).
188 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
189 * resolved along body-frame axes, averaged over time interval and
190 * expressed in meters per squared second (m/s^2).
191 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
192 * resolved along body-frame axes, averaged over time interval and
193 * expressed in meters per squared second (m/s^2).
194 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
195 * resolved along body-frame axes, averaged over time interval and
196 * expressed in radians per second (rad/s).
197 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
198 * resolved along body-frame axes, averaged over time interval and
199 * expressed in radians per second (rad/s).
200 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
201 * resolved along body-frame axes, averaged over time interval and
202 * expressed in radians per second (rad/s).
203 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
204 * @param result instance where new estimated NED frame containing new body position,
205 * velocity and coordinate transformation matrix will be stored.
206 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
207 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
208 * body-to-NED-frame coordinate transformation matrix are
209 * invalid.
210 */
211 public void navigate(
212 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
213 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
214 final double fx, final double fy, final double fz,
215 final double angularRateX, final double angularRateY, final double angularRateZ,
216 final double accuracyThreshold, final NEDFrame result)
217 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
218 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
219 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
220 }
221
222 /**
223 * Runs precision local-navigation-frame inertial navigation equations.
224 * NOTE: only the attitude update and specific force frame transformation
225 * phases are precise.
226 *
227 * @param timeInterval time interval between epochs.
228 * @param oldLatitude previous latitude expressed in radians (rad).
229 * @param oldLongitude previous longitude expressed in radians (rad).
230 * @param oldHeight previous height expressed in meters (m).
231 * @param oldC previous body-to-NED coordinate transformation.
232 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
233 * resolved along NED-frame axes and expressed in meters per second (m/s).
234 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
235 * resolved along NED-frame axes and expressed in meters per second (m/s).
236 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
237 * resolved along NED-frame axes and expressed in meters per second (m/s).
238 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
239 * resolved along body-frame axes, averaged over time interval and
240 * expressed in meters per squared second (m/s^2).
241 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
242 * resolved along body-frame axes, averaged over time interval and
243 * expressed in meters per squared second (m/s^2).
244 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
245 * resolved along body-frame axes, averaged over time interval and
246 * expressed in meters per squared second (m/s^2).
247 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
248 * resolved along body-frame axes, averaged over time interval and
249 * expressed in radians per second (rad/s).
250 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
251 * resolved along body-frame axes, averaged over time interval and
252 * expressed in radians per second (rad/s).
253 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
254 * resolved along body-frame axes, averaged over time interval and
255 * expressed in radians per second (rad/s).
256 * @param result instance where new estimated NED frame containing new body position,
257 * velocity and coordinate transformation matrix will be stored.
258 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
259 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
260 * body-to-NED-frame coordinate transformation matrix are
261 * invalid.
262 */
263 public void navigate(
264 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
265 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
266 final double fx, final double fy, final double fz,
267 final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result)
268 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
269 navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
270 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
271 }
272
273 /**
274 * Runs precision local-navigation-frame inertial navigation equations.
275 * NOTE: only the attitude update and specific force frame transformation
276 * phases are precise.
277 *
278 * @param timeInterval time interval between epochs expressed in seconds (s).
279 * @param oldPosition previous curvilinear position expressed in terms of latitude,
280 * longitude and height.
281 * @param oldC previous body-to-NED coordinate transformation.
282 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
283 * resolved along NED-frame axes and expressed in meters per second (m/s).
284 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
285 * resolved along NED-frame axes and expressed in meters per second (m/s).
286 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
287 * resolved along NED-frame axes and expressed in meters per second (m/s).
288 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
289 * resolved along body-frame axes, averaged over time interval and
290 * expressed in meters per squared second (m/s^2).
291 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
292 * resolved along body-frame axes, averaged over time interval and
293 * expressed in meters per squared second (m/s^2).
294 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
295 * resolved along body-frame axes, averaged over time interval and
296 * expressed in meters per squared second (m/s^2).
297 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
298 * resolved along body-frame axes, averaged over time interval and
299 * expressed in radians per second (rad/s).
300 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
301 * resolved along body-frame axes, averaged over time interval and
302 * expressed in radians per second (rad/s).
303 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
304 * resolved along body-frame axes, averaged over time interval and
305 * expressed in radians per second (rad/s).
306 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
307 * @param result instance where new estimated NED frame containing new body position,
308 * velocity and coordinate transformation matrix will be stored.
309 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
310 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
311 * body-to-NED-frame coordinate transformation matrix are
312 * invalid.
313 */
314 public void navigate(
315 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
316 final double oldVn, final double oldVe, final double oldVd,
317 final double fx, final double fy, final double fz,
318 final double angularRateX, final double angularRateY, final double angularRateZ,
319 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
320 InvalidSourceAndDestinationFrameTypeException {
321 navigateNED(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
322 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
323 }
324
325 /**
326 * Runs precision local-navigation-frame inertial navigation equations.
327 * NOTE: only the attitude update and specific force frame transformation
328 * phases are precise.
329 *
330 * @param timeInterval time interval between epochs expressed in seconds (s).
331 * @param oldPosition previous curvilinear position expressed in terms of latitude,
332 * longitude and height.
333 * @param oldC previous body-to-NED coordinate transformation.
334 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
335 * resolved along NED-frame axes and expressed in meters per second (m/s).
336 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
337 * resolved along NED-frame axes and expressed in meters per second (m/s).
338 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
339 * resolved along NED-frame axes and expressed in meters per second (m/s).
340 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
341 * resolved along body-frame axes, averaged over time interval and
342 * expressed in meters per squared second (m/s^2).
343 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
344 * resolved along body-frame axes, averaged over time interval and
345 * expressed in meters per squared second (m/s^2).
346 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
347 * resolved along body-frame axes, averaged over time interval and
348 * expressed in meters per squared second (m/s^2).
349 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
350 * resolved along body-frame axes, averaged over time interval and
351 * expressed in radians per second (rad/s).
352 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
353 * resolved along body-frame axes, averaged over time interval and
354 * expressed in radians per second (rad/s).
355 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
356 * resolved along body-frame axes, averaged over time interval and
357 * expressed in radians per second (rad/s).
358 * @param result instance where new estimated NED frame containing new body position,
359 * velocity and coordinate transformation matrix will be stored.
360 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
361 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
362 * body-to-NED-frame coordinate transformation matrix are
363 * invalid.
364 */
365 public void navigate(
366 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
367 final double oldVn, final double oldVe, final double oldVd,
368 final double fx, final double fy, final double fz,
369 final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result)
370 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
371 navigate(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
372 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
373 }
374
375 /**
376 * Runs precision local-navigation-frame inertial navigation equations.
377 * NOTE: only the attitude update and specific force frame transformation
378 * phases are precise.
379 *
380 * @param timeInterval time interval between epochs.
381 * @param oldPosition previous curvilinear position expressed in terms of latitude,
382 * longitude and height.
383 * @param oldC previous body-to-NED coordinate transformation.
384 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
385 * resolved along NED-frame axes and expressed in meters per second (m/s).
386 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
387 * resolved along NED-frame axes and expressed in meters per second (m/s).
388 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
389 * resolved along NED-frame axes and expressed in meters per second (m/s).
390 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
391 * resolved along body-frame axes, averaged over time interval and
392 * expressed in meters per squared second (m/s^2).
393 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
394 * resolved along body-frame axes, averaged over time interval and
395 * expressed in meters per squared second (m/s^2).
396 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
397 * resolved along body-frame axes, averaged over time interval and
398 * expressed in meters per squared second (m/s^2).
399 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
400 * resolved along body-frame axes, averaged over time interval and
401 * expressed in radians per second (rad/s).
402 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
403 * resolved along body-frame axes, averaged over time interval and
404 * expressed in radians per second (rad/s).
405 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
406 * resolved along body-frame axes, averaged over time interval and
407 * expressed in radians per second (rad/s).
408 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
409 * @param result instance where new estimated NED frame containing new body position,
410 * velocity and coordinate transformation matrix will be stored.
411 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
412 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
413 * body-to-NED-frame coordinate transformation matrix are
414 * invalid.
415 */
416 public void navigate(
417 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
418 final double oldVn, final double oldVe, final double oldVd,
419 final double fx, final double fy, final double fz,
420 final double angularRateX, final double angularRateY, final double angularRateZ,
421 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
422 InvalidSourceAndDestinationFrameTypeException {
423 navigateNED(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
424 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
425 }
426
427
428 /**
429 * Runs precision local-navigation-frame inertial navigation equations.
430 * NOTE: only the attitude update and specific force frame transformation
431 * phases are precise.
432 *
433 * @param timeInterval time interval between epochs.
434 * @param oldPosition previous curvilinear position expressed in terms of latitude,
435 * longitude and height.
436 * @param oldC previous body-to-NED coordinate transformation.
437 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
438 * resolved along NED-frame axes and expressed in meters per second (m/s).
439 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
440 * resolved along NED-frame axes and expressed in meters per second (m/s).
441 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
442 * resolved along NED-frame axes and expressed in meters per second (m/s).
443 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
444 * resolved along body-frame axes, averaged over time interval and
445 * expressed in meters per squared second (m/s^2).
446 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
447 * resolved along body-frame axes, averaged over time interval and
448 * expressed in meters per squared second (m/s^2).
449 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
450 * resolved along body-frame axes, averaged over time interval and
451 * expressed in meters per squared second (m/s^2).
452 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
453 * resolved along body-frame axes, averaged over time interval and
454 * expressed in radians per second (rad/s).
455 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
456 * resolved along body-frame axes, averaged over time interval and
457 * expressed in radians per second (rad/s).
458 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
459 * resolved along body-frame axes, averaged over time interval and
460 * expressed in radians per second (rad/s).
461 * @param result instance where new estimated NED frame containing new body position,
462 * velocity and coordinate transformation matrix will be stored.
463 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
464 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
465 * body-to-NED-frame coordinate transformation matrix are
466 * invalid.
467 */
468 public void navigate(
469 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
470 final double oldVn, final double oldVe, final double oldVd,
471 final double fx, final double fy, final double fz,
472 final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result)
473 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
474 navigate(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
475 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
476 }
477
478 /**
479 * Runs precision local-navigation-frame inertial navigation equations.
480 * NOTE: only the attitude update and specific force frame transformation
481 * phases are precise.
482 *
483 * @param timeInterval time interval between epochs expressed in seconds (s).
484 * @param oldLatitude previous latitude expressed in radians (rad).
485 * @param oldLongitude previous longitude expressed in radians (rad).
486 * @param oldHeight previous height expressed in meters (m).
487 * @param oldC previous body-to-NED coordinate transformation.
488 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
489 * along north, east and down axes.
490 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
491 * resolved along body-frame axes, averaged over time interval and
492 * expressed in meters per squared second (m/s^2).
493 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
494 * resolved along body-frame axes, averaged over time interval and
495 * expressed in meters per squared second (m/s^2).
496 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
497 * resolved along body-frame axes, averaged over time interval and
498 * expressed in meters per squared second (m/s^2).
499 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
500 * resolved along body-frame axes, averaged over time interval and
501 * expressed in radians per second (rad/s).
502 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
503 * resolved along body-frame axes, averaged over time interval and
504 * expressed in radians per second (rad/s).
505 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
506 * resolved along body-frame axes, averaged over time interval and
507 * expressed in radians per second (rad/s).
508 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
509 * @param result instance where new estimated NED frame containing new body position,
510 * velocity and coordinate transformation matrix will be stored.
511 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
512 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
513 * body-to-NED-frame coordinate transformation matrix are
514 * invalid.
515 */
516 public void navigate(
517 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
518 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
519 final double fx, final double fy, final double fz,
520 final double angularRateX, final double angularRateY, final double angularRateZ,
521 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
522 InvalidSourceAndDestinationFrameTypeException {
523 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
524 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
525 }
526
527 /**
528 * Runs precision local-navigation-frame inertial navigation equations.
529 * NOTE: only the attitude update and specific force frame transformation
530 * phases are precise.
531 *
532 * @param timeInterval time interval between epochs expressed in seconds (s).
533 * @param oldLatitude previous latitude expressed in radians (rad).
534 * @param oldLongitude previous longitude expressed in radians (rad).
535 * @param oldHeight previous height expressed in meters (m).
536 * @param oldC previous body-to-NED coordinate transformation.
537 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
538 * along north, east and down axes.
539 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
540 * resolved along body-frame axes, averaged over time interval and
541 * expressed in meters per squared second (m/s^2).
542 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
543 * resolved along body-frame axes, averaged over time interval and
544 * expressed in meters per squared second (m/s^2).
545 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
546 * resolved along body-frame axes, averaged over time interval and
547 * expressed in meters per squared second (m/s^2).
548 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
549 * resolved along body-frame axes, averaged over time interval and
550 * expressed in radians per second (rad/s).
551 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
552 * resolved along body-frame axes, averaged over time interval and
553 * expressed in radians per second (rad/s).
554 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
555 * resolved along body-frame axes, averaged over time interval and
556 * expressed in radians per second (rad/s).
557 * @param result instance where new estimated NED frame containing new body position,
558 * velocity and coordinate transformation matrix will be stored.
559 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
560 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
561 * body-to-NED-frame coordinate transformation matrix are
562 * invalid.
563 */
564 public void navigate(
565 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
566 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
567 final double fx, final double fy, final double fz,
568 final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result)
569 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
570 navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
571 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
572 }
573
574 /**
575 * Runs precision local-navigation-frame inertial navigation equations.
576 * NOTE: only the attitude update and specific force frame transformation
577 * phases are precise.
578 *
579 * @param timeInterval time interval between epochs.
580 * @param oldLatitude previous latitude expressed in radians (rad).
581 * @param oldLongitude previous longitude expressed in radians (rad).
582 * @param oldHeight previous height expressed in meters (m).
583 * @param oldC previous body-to-NED coordinate transformation.
584 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
585 * along north, east and down axes.
586 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
587 * resolved along body-frame axes, averaged over time interval and
588 * expressed in meters per squared second (m/s^2).
589 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
590 * resolved along body-frame axes, averaged over time interval and
591 * expressed in meters per squared second (m/s^2).
592 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
593 * resolved along body-frame axes, averaged over time interval and
594 * expressed in meters per squared second (m/s^2).
595 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
596 * resolved along body-frame axes, averaged over time interval and
597 * expressed in radians per second (rad/s).
598 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
599 * resolved along body-frame axes, averaged over time interval and
600 * expressed in radians per second (rad/s).
601 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
602 * resolved along body-frame axes, averaged over time interval and
603 * expressed in radians per second (rad/s).
604 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
605 * @param result instance where new estimated NED frame containing new body position,
606 * velocity and coordinate transformation matrix will be stored.
607 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
608 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
609 * body-to-NED-frame coordinate transformation matrix are
610 * invalid.
611 */
612 public void navigate(
613 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
614 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
615 final double fx, final double fy, final double fz,
616 final double angularRateX, final double angularRateY, final double angularRateZ,
617 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
618 InvalidSourceAndDestinationFrameTypeException {
619 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
620 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
621 }
622
623 /**
624 * Runs precision local-navigation-frame inertial navigation equations.
625 * NOTE: only the attitude update and specific force frame transformation
626 * phases are precise.
627 *
628 * @param timeInterval time interval between epochs.
629 * @param oldLatitude previous latitude expressed in radians (rad).
630 * @param oldLongitude previous longitude expressed in radians (rad).
631 * @param oldHeight previous height expressed in meters (m).
632 * @param oldC previous body-to-NED coordinate transformation.
633 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
634 * along north, east and down axes.
635 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
636 * resolved along body-frame axes, averaged over time interval and
637 * expressed in meters per squared second (m/s^2).
638 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
639 * resolved along body-frame axes, averaged over time interval and
640 * expressed in meters per squared second (m/s^2).
641 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
642 * resolved along body-frame axes, averaged over time interval and
643 * expressed in meters per squared second (m/s^2).
644 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
645 * resolved along body-frame axes, averaged over time interval and
646 * expressed in radians per second (rad/s).
647 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
648 * resolved along body-frame axes, averaged over time interval and
649 * expressed in radians per second (rad/s).
650 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
651 * resolved along body-frame axes, averaged over time interval and
652 * expressed in radians per second (rad/s).
653 * @param result instance where new estimated NED frame containing new body position,
654 * velocity and coordinate transformation matrix will be stored.
655 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
656 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
657 * body-to-NED-frame coordinate transformation matrix are
658 * invalid.
659 */
660 public void navigate(
661 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
662 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
663 final double fx, final double fy, final double fz,
664 final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result)
665 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
666 navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
667 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
668 }
669
670 /**
671 * Runs precision local-navigation-frame inertial navigation equations.
672 * NOTE: only the attitude update and specific force frame transformation
673 * phases are precise.
674 *
675 * @param timeInterval time interval between epochs expressed in seconds (s).
676 * @param oldPosition previous curvilinear position expressed in terms of latitude,
677 * longitude and height.
678 * @param oldC previous body-to-NED coordinate transformation.
679 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
680 * along north, east and down axes.
681 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
682 * resolved along body-frame axes, averaged over time interval and
683 * expressed in meters per squared second (m/s^2).
684 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
685 * resolved along body-frame axes, averaged over time interval and
686 * expressed in meters per squared second (m/s^2).
687 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
688 * resolved along body-frame axes, averaged over time interval and
689 * expressed in meters per squared second (m/s^2).
690 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
691 * resolved along body-frame axes, averaged over time interval and
692 * expressed in radians per second (rad/s).
693 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
694 * resolved along body-frame axes, averaged over time interval and
695 * expressed in radians per second (rad/s).
696 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
697 * resolved along body-frame axes, averaged over time interval and
698 * expressed in radians per second (rad/s).
699 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
700 * @param result instance where new estimated NED frame containing new body position,
701 * velocity and coordinate transformation matrix will be stored.
702 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
703 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
704 * body-to-NED-frame coordinate transformation matrix are
705 * invalid.
706 */
707 public void navigate(
708 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
709 final NEDVelocity oldVelocity, final double fx, final double fy, final double fz,
710 final double angularRateX, final double angularRateY, final double angularRateZ,
711 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
712 InvalidSourceAndDestinationFrameTypeException {
713 navigateNED(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
714 accuracyThreshold, result);
715 }
716
717 /**
718 * Runs precision local-navigation-frame inertial navigation equations.
719 * NOTE: only the attitude update and specific force frame transformation
720 * phases are precise.
721 *
722 * @param timeInterval time interval between epochs expressed in seconds (s).
723 * @param oldPosition previous curvilinear position expressed in terms of latitude,
724 * longitude and height.
725 * @param oldC previous body-to-NED coordinate transformation.
726 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
727 * along north, east and down axes.
728 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
729 * resolved along body-frame axes, averaged over time interval and
730 * expressed in meters per squared second (m/s^2).
731 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
732 * resolved along body-frame axes, averaged over time interval and
733 * expressed in meters per squared second (m/s^2).
734 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
735 * resolved along body-frame axes, averaged over time interval and
736 * expressed in meters per squared second (m/s^2).
737 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
738 * resolved along body-frame axes, averaged over time interval and
739 * expressed in radians per second (rad/s).
740 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
741 * resolved along body-frame axes, averaged over time interval and
742 * expressed in radians per second (rad/s).
743 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
744 * resolved along body-frame axes, averaged over time interval and
745 * expressed in radians per second (rad/s).
746 * @param result instance where new estimated NED frame containing new body position,
747 * velocity and coordinate transformation matrix will be stored.
748 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
749 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
750 * body-to-NED-frame coordinate transformation matrix are
751 * invalid.
752 */
753 public void navigate(
754 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
755 final NEDVelocity oldVelocity, final double fx, final double fy, final double fz,
756 final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result)
757 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
758 navigate(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
759 DEFAULT_ACCURACY_THRESHOLD, result);
760 }
761
762 /**
763 * Runs precision local-navigation-frame inertial navigation equations.
764 * NOTE: only the attitude update and specific force frame transformation
765 * phases are precise.
766 *
767 * @param timeInterval time interval between epochs.
768 * @param oldPosition previous curvilinear position expressed in terms of latitude,
769 * longitude and height.
770 * @param oldC previous body-to-NED coordinate transformation.
771 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
772 * along north, east and down axes.
773 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
774 * resolved along body-frame axes, averaged over time interval and
775 * expressed in meters per squared second (m/s^2).
776 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
777 * resolved along body-frame axes, averaged over time interval and
778 * expressed in meters per squared second (m/s^2).
779 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
780 * resolved along body-frame axes, averaged over time interval and
781 * expressed in meters per squared second (m/s^2).
782 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
783 * resolved along body-frame axes, averaged over time interval and
784 * expressed in radians per second (rad/s).
785 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
786 * resolved along body-frame axes, averaged over time interval and
787 * expressed in radians per second (rad/s).
788 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
789 * resolved along body-frame axes, averaged over time interval and
790 * expressed in radians per second (rad/s).
791 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
792 * @param result instance where new estimated NED frame containing new body position,
793 * velocity and coordinate transformation matrix will be stored.
794 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
795 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
796 * body-to-NED-frame coordinate transformation matrix are
797 * invalid.
798 */
799 public void navigate(
800 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
801 final NEDVelocity oldVelocity, final double fx, final double fy, final double fz,
802 final double angularRateX, final double angularRateY, final double angularRateZ,
803 final double accuracyThreshold, final NEDFrame result)
804 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
805 navigateNED(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
806 accuracyThreshold, result);
807 }
808
809 /**
810 * Runs precision local-navigation-frame inertial navigation equations.
811 * NOTE: only the attitude update and specific force frame transformation
812 * phases are precise.
813 *
814 * @param timeInterval time interval between epochs.
815 * @param oldPosition previous curvilinear position expressed in terms of latitude,
816 * longitude and height.
817 * @param oldC previous body-to-NED coordinate transformation.
818 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
819 * along north, east and down axes.
820 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
821 * resolved along body-frame axes, averaged over time interval and
822 * expressed in meters per squared second (m/s^2).
823 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
824 * resolved along body-frame axes, averaged over time interval and
825 * expressed in meters per squared second (m/s^2).
826 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
827 * resolved along body-frame axes, averaged over time interval and
828 * expressed in meters per squared second (m/s^2).
829 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
830 * resolved along body-frame axes, averaged over time interval and
831 * expressed in radians per second (rad/s).
832 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
833 * resolved along body-frame axes, averaged over time interval and
834 * expressed in radians per second (rad/s).
835 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
836 * resolved along body-frame axes, averaged over time interval and
837 * expressed in radians per second (rad/s).
838 * @param result instance where new estimated NED frame containing new body position,
839 * velocity and coordinate transformation matrix will be stored.
840 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
841 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
842 * body-to-NED-frame coordinate transformation matrix are
843 * invalid.
844 */
845 public void navigate(
846 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
847 final NEDVelocity oldVelocity, final double fx, final double fy, final double fz,
848 final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result)
849 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
850 navigate(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
851 DEFAULT_ACCURACY_THRESHOLD, result);
852 }
853
854 /**
855 * Runs precision local-navigation-frame inertial navigation equations.
856 * NOTE: only the attitude update and specific force frame transformation
857 * phases are precise.
858 *
859 * @param timeInterval time interval between epochs expressed in seconds (s).
860 * @param oldLatitude previous latitude expressed in radians (rad).
861 * @param oldLongitude previous longitude expressed in radians (rad).
862 * @param oldHeight previous height expressed in meters (m).
863 * @param oldC previous body-to-NED coordinate transformation.
864 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
865 * resolved along NED-frame axes and expressed in meters per second (m/s).
866 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
867 * resolved along NED-frame axes and expressed in meters per second (m/s).
868 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
869 * resolved along NED-frame axes and expressed in meters per second (m/s).
870 * @param kinematics body kinematics containing specific forces and angular rates applied to
871 * the body.
872 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
873 * @param result instance where new estimated NED frame containing new body position,
874 * velocity and coordinate transformation matrix will be stored.
875 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
876 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
877 * body-to-NED-frame coordinate transformation matrix are
878 * invalid.
879 */
880 public void navigate(
881 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
882 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
883 final BodyKinematics kinematics, final double accuracyThreshold, final NEDFrame result)
884 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
885 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, kinematics,
886 accuracyThreshold, result);
887 }
888
889 /**
890 * Runs precision local-navigation-frame inertial navigation equations.
891 * NOTE: only the attitude update and specific force frame transformation
892 * phases are precise.
893 *
894 * @param timeInterval time interval between epochs expressed in seconds (s).
895 * @param oldLatitude previous latitude expressed in radians (rad).
896 * @param oldLongitude previous longitude expressed in radians (rad).
897 * @param oldHeight previous height expressed in meters (m).
898 * @param oldC previous body-to-NED coordinate transformation.
899 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
900 * resolved along NED-frame axes and expressed in meters per second (m/s).
901 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
902 * resolved along NED-frame axes and expressed in meters per second (m/s).
903 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
904 * resolved along NED-frame axes and expressed in meters per second (m/s).
905 * @param kinematics body kinematics containing specific forces and angular rates applied to
906 * the body.
907 * @param result instance where new estimated NED frame containing new body position,
908 * velocity and coordinate transformation matrix will be stored.
909 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
910 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
911 * body-to-NED-frame coordinate transformation matrix are
912 * invalid.
913 */
914 public void navigate(
915 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
916 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
917 final BodyKinematics kinematics, final NEDFrame result) throws InertialNavigatorException,
918 InvalidSourceAndDestinationFrameTypeException {
919 navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, kinematics,
920 DEFAULT_ACCURACY_THRESHOLD, result);
921 }
922
923
924 /**
925 * Runs precision local-navigation-frame inertial navigation equations.
926 * NOTE: only the attitude update and specific force frame transformation
927 * phases are precise.
928 *
929 * @param timeInterval time interval between epochs.
930 * @param oldLatitude previous latitude expressed in radians (rad).
931 * @param oldLongitude previous longitude expressed in radians (rad).
932 * @param oldHeight previous height expressed in meters (m).
933 * @param oldC previous body-to-NED coordinate transformation.
934 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
935 * resolved along NED-frame axes and expressed in meters per second (m/s).
936 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
937 * resolved along NED-frame axes and expressed in meters per second (m/s).
938 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
939 * resolved along NED-frame axes and expressed in meters per second (m/s).
940 * @param kinematics body kinematics containing specific forces and angular rates applied to
941 * the body.
942 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
943 * @param result instance where new estimated NED frame containing new body position,
944 * velocity and coordinate transformation matrix will be stored.
945 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
946 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
947 * body-to-NED-frame coordinate transformation matrix are
948 * invalid.
949 */
950 public void navigate(
951 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
952 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
953 final BodyKinematics kinematics, final double accuracyThreshold, final NEDFrame result)
954 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
955 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, kinematics,
956 accuracyThreshold, result);
957 }
958
959 /**
960 * Runs precision local-navigation-frame inertial navigation equations.
961 * NOTE: only the attitude update and specific force frame transformation
962 * phases are precise.
963 *
964 * @param timeInterval time interval between epochs.
965 * @param oldLatitude previous latitude expressed in radians (rad).
966 * @param oldLongitude previous longitude expressed in radians (rad).
967 * @param oldHeight previous height expressed in meters (m).
968 * @param oldC previous body-to-NED coordinate transformation.
969 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
970 * resolved along NED-frame axes and expressed in meters per second (m/s).
971 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
972 * resolved along NED-frame axes and expressed in meters per second (m/s).
973 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
974 * resolved along NED-frame axes and expressed in meters per second (m/s).
975 * @param kinematics body kinematics containing specific forces and angular rates applied to
976 * the body.
977 * @param result instance where new estimated NED frame containing new body position,
978 * velocity and coordinate transformation matrix will be stored.
979 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
980 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
981 * body-to-NED-frame coordinate transformation matrix are
982 * invalid.
983 */
984 public void navigate(
985 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
986 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
987 final BodyKinematics kinematics, final NEDFrame result) throws InertialNavigatorException,
988 InvalidSourceAndDestinationFrameTypeException {
989 navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, kinematics,
990 DEFAULT_ACCURACY_THRESHOLD, result);
991 }
992
993 /**
994 * Runs precision local-navigation-frame inertial navigation equations.
995 * NOTE: only the attitude update and specific force frame transformation
996 * phases are precise.
997 *
998 * @param timeInterval time interval between epochs expressed in seconds (s).
999 * @param oldPosition previous curvilinear position expressed in terms of latitude,
1000 * longitude and height.
1001 * @param oldC previous body-to-NED coordinate transformation.
1002 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
1003 * resolved along NED-frame axes and expressed in meters per second (m/s).
1004 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
1005 * resolved along NED-frame axes and expressed in meters per second (m/s).
1006 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
1007 * resolved along NED-frame axes and expressed in meters per second (m/s).
1008 * @param kinematics body kinematics containing specific forces and angular rates applied to
1009 * the body.
1010 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
1011 * @param result instance where new estimated NED frame containing new body position,
1012 * velocity and coordinate transformation matrix will be stored.
1013 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
1014 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
1015 * body-to-NED-frame coordinate transformation matrix are
1016 * invalid.
1017 */
1018 public void navigate(
1019 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
1020 final double oldVn, final double oldVe, final double oldVd, final BodyKinematics kinematics,
1021 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
1022 InvalidSourceAndDestinationFrameTypeException {
1023 navigateNED(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, kinematics, accuracyThreshold, result);
1024 }
1025
1026 /**
1027 * Runs precision local-navigation-frame inertial navigation equations.
1028 * NOTE: only the attitude update and specific force frame transformation
1029 * phases are precise.
1030 *
1031 * @param timeInterval time interval between epochs expressed in seconds (s).
1032 * @param oldPosition previous curvilinear position expressed in terms of latitude,
1033 * longitude and height.
1034 * @param oldC previous body-to-NED coordinate transformation.
1035 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
1036 * resolved along NED-frame axes and expressed in meters per second (m/s).
1037 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
1038 * resolved along NED-frame axes and expressed in meters per second (m/s).
1039 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
1040 * resolved along NED-frame axes and expressed in meters per second (m/s).
1041 * @param kinematics body kinematics containing specific forces and angular rates applied to
1042 * the body.
1043 * @param result instance where new estimated NED frame containing new body position,
1044 * velocity and coordinate transformation matrix will be stored.
1045 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
1046 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
1047 * body-to-NED-frame coordinate transformation matrix are
1048 * invalid.
1049 */
1050 public void navigate(
1051 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
1052 final double oldVn, final double oldVe, final double oldVd, final BodyKinematics kinematics,
1053 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
1054 navigate(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, kinematics, DEFAULT_ACCURACY_THRESHOLD, result);
1055 }
1056
1057 /**
1058 * Runs precision local-navigation-frame inertial navigation equations.
1059 * NOTE: only the attitude update and specific force frame transformation
1060 * phases are precise.
1061 *
1062 * @param timeInterval time interval between epochs.
1063 * @param oldPosition previous curvilinear position expressed in terms of latitude,
1064 * longitude and height.
1065 * @param oldC previous body-to-NED coordinate transformation.
1066 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
1067 * resolved along NED-frame axes and expressed in meters per second (m/s).
1068 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
1069 * resolved along NED-frame axes and expressed in meters per second (m/s).
1070 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
1071 * resolved along NED-frame axes and expressed in meters per second (m/s).
1072 * @param kinematics body kinematics containing specific forces and angular rates applied to
1073 * the body.
1074 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
1075 * @param result instance where new estimated NED frame containing new body position,
1076 * velocity and coordinate transformation matrix will be stored.
1077 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
1078 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
1079 * body-to-NED-frame coordinate transformation matrix are
1080 * invalid.
1081 */
1082 public void navigate(
1083 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
1084 final double oldVn, final double oldVe, final double oldVd, final BodyKinematics kinematics,
1085 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
1086 InvalidSourceAndDestinationFrameTypeException {
1087 navigateNED(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, kinematics, accuracyThreshold, result);
1088 }
1089
1090 /**
1091 * Runs precision local-navigation-frame inertial navigation equations.
1092 * NOTE: only the attitude update and specific force frame transformation
1093 * phases are precise.
1094 *
1095 * @param timeInterval time interval between epochs.
1096 * @param oldPosition previous curvilinear position expressed in terms of latitude,
1097 * longitude and height.
1098 * @param oldC previous body-to-NED coordinate transformation.
1099 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
1100 * resolved along NED-frame axes and expressed in meters per second (m/s).
1101 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
1102 * resolved along NED-frame axes and expressed in meters per second (m/s).
1103 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
1104 * resolved along NED-frame axes and expressed in meters per second (m/s).
1105 * @param kinematics body kinematics containing specific forces and angular rates applied to
1106 * the body.
1107 * @param result instance where new estimated NED frame containing new body position,
1108 * velocity and coordinate transformation matrix will be stored.
1109 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
1110 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
1111 * body-to-NED-frame coordinate transformation matrix are
1112 * invalid.
1113 */
1114 public void navigate(
1115 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
1116 final double oldVn, final double oldVe, final double oldVd, final BodyKinematics kinematics,
1117 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
1118 navigate(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, kinematics, DEFAULT_ACCURACY_THRESHOLD, result);
1119 }
1120
1121 /**
1122 * Runs precision local-navigation-frame inertial navigation equations.
1123 * NOTE: only the attitude update and specific force frame transformation
1124 * phases are precise.
1125 *
1126 * @param timeInterval time interval between epochs expressed in seconds (s).
1127 * @param oldLatitude previous latitude expressed in radians (rad).
1128 * @param oldLongitude previous longitude expressed in radians (rad).
1129 * @param oldHeight previous height expressed in meters (m).
1130 * @param oldC previous body-to-NED coordinate transformation.
1131 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
1132 * along north, east and down axes.
1133 * @param kinematics body kinematics containing specific forces and angular rates applied to
1134 * the body.
1135 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
1136 * @param result instance where new estimated NED frame containing new body position,
1137 * velocity and coordinate transformation matrix will be stored.
1138 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
1139 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
1140 * body-to-NED-frame coordinate transformation matrix are
1141 * invalid.
1142 */
1143 public void navigate(
1144 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
1145 final CoordinateTransformation oldC, final NEDVelocity oldVelocity, final BodyKinematics kinematics,
1146 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
1147 InvalidSourceAndDestinationFrameTypeException {
1148 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, kinematics,
1149 accuracyThreshold, result);
1150 }
1151
1152 /**
1153 * Runs precision local-navigation-frame inertial navigation equations.
1154 * NOTE: only the attitude update and specific force frame transformation
1155 * phases are precise.
1156 *
1157 * @param timeInterval time interval between epochs expressed in seconds (s).
1158 * @param oldLatitude previous latitude expressed in radians (rad).
1159 * @param oldLongitude previous longitude expressed in radians (rad).
1160 * @param oldHeight previous height expressed in meters (m).
1161 * @param oldC previous body-to-NED coordinate transformation.
1162 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
1163 * along north, east and down axes.
1164 * @param kinematics body kinematics containing specific forces and angular rates applied to
1165 * the body.
1166 * @param result instance where new estimated NED frame containing new body position,
1167 * velocity and coordinate transformation matrix will be stored.
1168 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
1169 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
1170 * body-to-NED-frame coordinate transformation matrix are
1171 * invalid.
1172 */
1173 public void navigate(
1174 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
1175 final CoordinateTransformation oldC, final NEDVelocity oldVelocity, final BodyKinematics kinematics,
1176 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
1177 navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, kinematics,
1178 DEFAULT_ACCURACY_THRESHOLD, result);
1179 }
1180
1181 /**
1182 * Runs precision local-navigation-frame inertial navigation equations.
1183 * NOTE: only the attitude update and specific force frame transformation
1184 * phases are precise.
1185 *
1186 * @param timeInterval time interval between epochs.
1187 * @param oldLatitude previous latitude expressed in radians (rad).
1188 * @param oldLongitude previous longitude expressed in radians (rad).
1189 * @param oldHeight previous height expressed in meters (m).
1190 * @param oldC previous body-to-NED coordinate transformation.
1191 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
1192 * along north, east and down axes.
1193 * @param kinematics body kinematics containing specific forces and angular rates applied to
1194 * the body.
1195 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
1196 * @param result instance where new estimated NED frame containing new body position,
1197 * velocity and coordinate transformation matrix will be stored.
1198 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
1199 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
1200 * body-to-NED-frame coordinate transformation matrix are
1201 * invalid.
1202 */
1203 public void navigate(
1204 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
1205 final CoordinateTransformation oldC, final NEDVelocity oldVelocity, final BodyKinematics kinematics,
1206 final double accuracyThreshold, final NEDFrame result)
1207 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
1208 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, kinematics,
1209 accuracyThreshold, result);
1210 }
1211
1212 /**
1213 * Runs precision local-navigation-frame inertial navigation equations.
1214 * NOTE: only the attitude update and specific force frame transformation
1215 * phases are precise.
1216 *
1217 * @param timeInterval time interval between epochs.
1218 * @param oldLatitude previous latitude expressed in radians (rad).
1219 * @param oldLongitude previous longitude expressed in radians (rad).
1220 * @param oldHeight previous height expressed in meters (m).
1221 * @param oldC previous body-to-NED coordinate transformation.
1222 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
1223 * along north, east and down axes.
1224 * @param kinematics body kinematics containing specific forces and angular rates applied to
1225 * the body.
1226 * @param result instance where new estimated NED frame containing new body position,
1227 * velocity and coordinate transformation matrix will be stored.
1228 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
1229 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
1230 * body-to-NED-frame coordinate transformation matrix are
1231 * invalid.
1232 */
1233 public void navigate(
1234 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
1235 final CoordinateTransformation oldC, final NEDVelocity oldVelocity, final BodyKinematics kinematics,
1236 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
1237 navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, kinematics,
1238 DEFAULT_ACCURACY_THRESHOLD, result);
1239 }
1240
1241 /**
1242 * Runs precision local-navigation-frame inertial navigation equations.
1243 * NOTE: only the attitude update and specific force frame transformation
1244 * phases are precise.
1245 *
1246 * @param timeInterval time interval between epochs expressed in seconds (s).
1247 * @param oldPosition previous curvilinear position expressed in terms of latitude,
1248 * longitude and height.
1249 * @param oldC previous body-to-NED coordinate transformation.
1250 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
1251 * along north, east and down axes.
1252 * @param kinematics body kinematics containing specific forces and angular rates applied to
1253 * the body.
1254 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
1255 * @param result instance where new estimated NED frame containing new body position,
1256 * velocity and coordinate transformation matrix will be stored.
1257 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
1258 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
1259 * body-to-NED-frame coordinate transformation matrix are
1260 * invalid.
1261 */
1262 public void navigate(
1263 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
1264 final NEDVelocity oldVelocity, final BodyKinematics kinematics, final double accuracyThreshold,
1265 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
1266 navigateNED(timeInterval, oldPosition, oldC, oldVelocity, kinematics, accuracyThreshold, result);
1267 }
1268
1269 /**
1270 * Runs precision local-navigation-frame inertial navigation equations.
1271 * NOTE: only the attitude update and specific force frame transformation
1272 * phases are precise.
1273 *
1274 * @param timeInterval time interval between epochs expressed in seconds (s).
1275 * @param oldPosition previous curvilinear position expressed in terms of latitude,
1276 * longitude and height.
1277 * @param oldC previous body-to-NED coordinate transformation.
1278 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
1279 * along north, east and down axes.
1280 * @param kinematics body kinematics containing specific forces and angular rates applied to
1281 * the body.
1282 * @param result instance where new estimated NED frame containing new body position,
1283 * velocity and coordinate transformation matrix will be stored.
1284 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
1285 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
1286 * body-to-NED-frame coordinate transformation matrix are
1287 * invalid.
1288 */
1289 public void navigate(
1290 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
1291 final NEDVelocity oldVelocity, final BodyKinematics kinematics, final NEDFrame result)
1292 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
1293 navigate(timeInterval, oldPosition, oldC, oldVelocity, kinematics, DEFAULT_ACCURACY_THRESHOLD, result);
1294 }
1295
1296 /**
1297 * Runs precision local-navigation-frame inertial navigation equations.
1298 * NOTE: only the attitude update and specific force frame transformation
1299 * phases are precise.
1300 *
1301 * @param timeInterval time interval between epochs expressed in seconds (s).
1302 * @param oldPosition previous curvilinear position expressed in terms of latitude,
1303 * longitude and height.
1304 * @param oldC previous body-to-NED coordinate transformation.
1305 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
1306 * along north, east and down axes.
1307 * @param kinematics body kinematics containing specific forces and angular rates applied to
1308 * the body.
1309 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
1310 * @param result instance where new estimated NED frame containing new body position,
1311 * velocity and coordinate transformation matrix will be stored.
1312 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
1313 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
1314 * body-to-NED-frame coordinate transformation matrix are
1315 * invalid.
1316 */
1317 public void navigate(
1318 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
1319 final NEDVelocity oldVelocity, final BodyKinematics kinematics, final double accuracyThreshold,
1320 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
1321 navigateNED(timeInterval, oldPosition, oldC, oldVelocity, kinematics, accuracyThreshold, result);
1322 }
1323
1324 /**
1325 * Runs precision local-navigation-frame inertial navigation equations.
1326 * NOTE: only the attitude update and specific force frame transformation
1327 * phases are precise.
1328 *
1329 * @param timeInterval time interval between epochs expressed in seconds (s).
1330 * @param oldPosition previous curvilinear position expressed in terms of latitude,
1331 * longitude and height.
1332 * @param oldC previous body-to-NED coordinate transformation.
1333 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
1334 * along north, east and down axes.
1335 * @param kinematics body kinematics containing specific forces and angular rates applied to
1336 * the body.
1337 * @param result instance where new estimated NED frame containing new body position,
1338 * velocity and coordinate transformation matrix will be stored.
1339 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
1340 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
1341 * body-to-NED-frame coordinate transformation matrix are
1342 * invalid.
1343 */
1344 public void navigate(
1345 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
1346 final NEDVelocity oldVelocity, final BodyKinematics kinematics, final NEDFrame result)
1347 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
1348 navigate(timeInterval, oldPosition, oldC, oldVelocity, kinematics, DEFAULT_ACCURACY_THRESHOLD, result);
1349 }
1350
1351 /**
1352 * Runs precision local-navigation-frame inertial navigation equations.
1353 * NOTE: only the attitude update and specific force frame transformation
1354 * phases are precise.
1355 *
1356 * @param timeInterval time interval between epochs expressed in seconds (s).
1357 * @param oldLatitude previous latitude angle.
1358 * @param oldLongitude previous longitude angle.
1359 * @param oldHeight previous height.
1360 * @param oldC previous body-to-NED coordinate transformation.
1361 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
1362 * resolved along NED-frame axes and expressed in meters per second (m/s).
1363 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
1364 * resolved along NED-frame axes and expressed in meters per second (m/s).
1365 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
1366 * resolved along NED-frame axes and expressed in meters per second (m/s).
1367 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
1368 * resolved along body-frame axes, averaged over time interval and
1369 * expressed in meters per squared second (m/s^2).
1370 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
1371 * resolved along body-frame axes, averaged over time interval and
1372 * expressed in meters per squared second (m/s^2).
1373 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
1374 * resolved along body-frame axes, averaged over time interval and
1375 * expressed in meters per squared second (m/s^2).
1376 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
1377 * resolved along body-frame axes, averaged over time interval and
1378 * expressed in radians per second (rad/s).
1379 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
1380 * resolved along body-frame axes, averaged over time interval and
1381 * expressed in radians per second (rad/s).
1382 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
1383 * resolved along body-frame axes, averaged over time interval and
1384 * expressed in radians per second (rad/s).
1385 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
1386 * @param result instance where new estimated NED frame containing new body position,
1387 * velocity and coordinate transformation matrix will be stored.
1388 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
1389 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
1390 * body-to-NED-frame coordinate transformation matrix are
1391 * invalid.
1392 */
1393 public void navigate(
1394 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
1395 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
1396 final double fx, final double fy, final double fz,
1397 final double angularRateX, final double angularRateY, final double angularRateZ,
1398 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
1399 InvalidSourceAndDestinationFrameTypeException {
1400 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
1401 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
1402 }
1403
1404 /**
1405 * Runs precision local-navigation-frame inertial navigation equations.
1406 * NOTE: only the attitude update and specific force frame transformation
1407 * phases are precise.
1408 *
1409 * @param timeInterval time interval between epochs expressed in seconds (s).
1410 * @param oldLatitude previous latitude angle.
1411 * @param oldLongitude previous longitude angle.
1412 * @param oldHeight previous height.
1413 * @param oldC previous body-to-NED coordinate transformation.
1414 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
1415 * resolved along NED-frame axes and expressed in meters per second (m/s).
1416 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
1417 * resolved along NED-frame axes and expressed in meters per second (m/s).
1418 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
1419 * resolved along NED-frame axes and expressed in meters per second (m/s).
1420 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
1421 * resolved along body-frame axes, averaged over time interval and
1422 * expressed in meters per squared second (m/s^2).
1423 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
1424 * resolved along body-frame axes, averaged over time interval and
1425 * expressed in meters per squared second (m/s^2).
1426 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
1427 * resolved along body-frame axes, averaged over time interval and
1428 * expressed in meters per squared second (m/s^2).
1429 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
1430 * resolved along body-frame axes, averaged over time interval and
1431 * expressed in radians per second (rad/s).
1432 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
1433 * resolved along body-frame axes, averaged over time interval and
1434 * expressed in radians per second (rad/s).
1435 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
1436 * resolved along body-frame axes, averaged over time interval and
1437 * expressed in radians per second (rad/s).
1438 * @param result instance where new estimated NED frame containing new body position,
1439 * velocity and coordinate transformation matrix will be stored.
1440 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
1441 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
1442 * body-to-NED-frame coordinate transformation matrix are
1443 * invalid.
1444 */
1445 public void navigate(
1446 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
1447 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
1448 final double fx, final double fy, final double fz,
1449 final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result)
1450 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
1451 navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
1452 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
1453 }
1454
1455 /**
1456 * Runs precision local-navigation-frame inertial navigation equations.
1457 * NOTE: only the attitude update and specific force frame transformation
1458 * phases are precise.
1459 *
1460 * @param timeInterval time interval between epochs.
1461 * @param oldLatitude previous latitude angle.
1462 * @param oldLongitude previous longitude angle.
1463 * @param oldHeight previous height.
1464 * @param oldC previous body-to-NED coordinate transformation.
1465 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
1466 * resolved along NED-frame axes and expressed in meters per second (m/s).
1467 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
1468 * resolved along NED-frame axes and expressed in meters per second (m/s).
1469 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
1470 * resolved along NED-frame axes and expressed in meters per second (m/s).
1471 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
1472 * resolved along body-frame axes, averaged over time interval and
1473 * expressed in meters per squared second (m/s^2).
1474 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
1475 * resolved along body-frame axes, averaged over time interval and
1476 * expressed in meters per squared second (m/s^2).
1477 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
1478 * resolved along body-frame axes, averaged over time interval and
1479 * expressed in meters per squared second (m/s^2).
1480 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
1481 * resolved along body-frame axes, averaged over time interval and
1482 * expressed in radians per second (rad/s).
1483 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
1484 * resolved along body-frame axes, averaged over time interval and
1485 * expressed in radians per second (rad/s).
1486 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
1487 * resolved along body-frame axes, averaged over time interval and
1488 * expressed in radians per second (rad/s).
1489 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
1490 * @param result instance where new estimated NED frame containing new body position,
1491 * velocity and coordinate transformation matrix will be stored.
1492 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
1493 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
1494 * body-to-NED-frame coordinate transformation matrix are
1495 * invalid.
1496 */
1497 public void navigate(
1498 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
1499 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
1500 final double fx, final double fy, final double fz,
1501 final double angularRateX, final double angularRateY, final double angularRateZ,
1502 final double accuracyThreshold, final NEDFrame result)
1503 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
1504 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
1505 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
1506 }
1507
1508 /**
1509 * Runs precision local-navigation-frame inertial navigation equations.
1510 * NOTE: only the attitude update and specific force frame transformation
1511 * phases are precise.
1512 *
1513 * @param timeInterval time interval between epochs.
1514 * @param oldLatitude previous latitude angle.
1515 * @param oldLongitude previous longitude angle.
1516 * @param oldHeight previous height.
1517 * @param oldC previous body-to-NED coordinate transformation.
1518 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
1519 * resolved along NED-frame axes and expressed in meters per second (m/s).
1520 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
1521 * resolved along NED-frame axes and expressed in meters per second (m/s).
1522 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
1523 * resolved along NED-frame axes and expressed in meters per second (m/s).
1524 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
1525 * resolved along body-frame axes, averaged over time interval and
1526 * expressed in meters per squared second (m/s^2).
1527 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
1528 * resolved along body-frame axes, averaged over time interval and
1529 * expressed in meters per squared second (m/s^2).
1530 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
1531 * resolved along body-frame axes, averaged over time interval and
1532 * expressed in meters per squared second (m/s^2).
1533 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
1534 * resolved along body-frame axes, averaged over time interval and
1535 * expressed in radians per second (rad/s).
1536 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
1537 * resolved along body-frame axes, averaged over time interval and
1538 * expressed in radians per second (rad/s).
1539 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
1540 * resolved along body-frame axes, averaged over time interval and
1541 * expressed in radians per second (rad/s).
1542 * @param result instance where new estimated NED frame containing new body position,
1543 * velocity and coordinate transformation matrix will be stored.
1544 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
1545 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
1546 * body-to-NED-frame coordinate transformation matrix are
1547 * invalid.
1548 */
1549 public void navigate(
1550 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
1551 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
1552 final double fx, final double fy, final double fz,
1553 final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result)
1554 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
1555 navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
1556 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
1557 }
1558
1559 /**
1560 * Runs precision local-navigation-frame inertial navigation equations.
1561 * NOTE: only the attitude update and specific force frame transformation
1562 * phases are precise.
1563 *
1564 * @param timeInterval time interval between epochs expressed in seconds (s).
1565 * @param oldLatitude previous latitude angle.
1566 * @param oldLongitude previous longitude angle.
1567 * @param oldHeight previous height.
1568 * @param oldC previous body-to-NED coordinate transformation.
1569 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
1570 * along north, east and down axes.
1571 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
1572 * resolved along body-frame axes, averaged over time interval and
1573 * expressed in meters per squared second (m/s^2).
1574 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
1575 * resolved along body-frame axes, averaged over time interval and
1576 * expressed in meters per squared second (m/s^2).
1577 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
1578 * resolved along body-frame axes, averaged over time interval and
1579 * expressed in meters per squared second (m/s^2).
1580 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
1581 * resolved along body-frame axes, averaged over time interval and
1582 * expressed in radians per second (rad/s).
1583 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
1584 * resolved along body-frame axes, averaged over time interval and
1585 * expressed in radians per second (rad/s).
1586 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
1587 * resolved along body-frame axes, averaged over time interval and
1588 * expressed in radians per second (rad/s).
1589 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
1590 * @param result instance where new estimated NED frame containing new body position,
1591 * velocity and coordinate transformation matrix will be stored.
1592 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
1593 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
1594 * body-to-NED-frame coordinate transformation matrix are
1595 * invalid.
1596 */
1597 public void navigate(
1598 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
1599 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
1600 final double fx, final double fy, final double fz,
1601 final double angularRateX, final double angularRateY, final double angularRateZ,
1602 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
1603 InvalidSourceAndDestinationFrameTypeException {
1604 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
1605 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
1606 }
1607
1608 /**
1609 * Runs precision local-navigation-frame inertial navigation equations.
1610 * NOTE: only the attitude update and specific force frame transformation
1611 * phases are precise.
1612 *
1613 * @param timeInterval time interval between epochs expressed in seconds (s).
1614 * @param oldLatitude previous latitude angle.
1615 * @param oldLongitude previous longitude angle.
1616 * @param oldHeight previous height.
1617 * @param oldC previous body-to-NED coordinate transformation.
1618 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
1619 * along north, east and down axes.
1620 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
1621 * resolved along body-frame axes, averaged over time interval and
1622 * expressed in meters per squared second (m/s^2).
1623 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
1624 * resolved along body-frame axes, averaged over time interval and
1625 * expressed in meters per squared second (m/s^2).
1626 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
1627 * resolved along body-frame axes, averaged over time interval and
1628 * expressed in meters per squared second (m/s^2).
1629 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
1630 * resolved along body-frame axes, averaged over time interval and
1631 * expressed in radians per second (rad/s).
1632 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
1633 * resolved along body-frame axes, averaged over time interval and
1634 * expressed in radians per second (rad/s).
1635 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
1636 * resolved along body-frame axes, averaged over time interval and
1637 * expressed in radians per second (rad/s).
1638 * @param result instance where new estimated NED frame containing new body position,
1639 * velocity and coordinate transformation matrix will be stored.
1640 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
1641 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
1642 * body-to-NED-frame coordinate transformation matrix are
1643 * invalid.
1644 */
1645 public void navigate(
1646 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
1647 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
1648 final double fx, final double fy, final double fz,
1649 final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result)
1650 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
1651 navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
1652 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
1653 }
1654
1655 /**
1656 * Runs precision local-navigation-frame inertial navigation equations.
1657 * NOTE: only the attitude update and specific force frame transformation
1658 * phases are precise.
1659 *
1660 * @param timeInterval time interval between epochs.
1661 * @param oldLatitude previous latitude angle.
1662 * @param oldLongitude previous longitude angle.
1663 * @param oldHeight previous height.
1664 * @param oldC previous body-to-NED coordinate transformation.
1665 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
1666 * along north, east and down axes.
1667 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
1668 * resolved along body-frame axes, averaged over time interval and
1669 * expressed in meters per squared second (m/s^2).
1670 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
1671 * resolved along body-frame axes, averaged over time interval and
1672 * expressed in meters per squared second (m/s^2).
1673 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
1674 * resolved along body-frame axes, averaged over time interval and
1675 * expressed in meters per squared second (m/s^2).
1676 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
1677 * resolved along body-frame axes, averaged over time interval and
1678 * expressed in radians per second (rad/s).
1679 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
1680 * resolved along body-frame axes, averaged over time interval and
1681 * expressed in radians per second (rad/s).
1682 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
1683 * resolved along body-frame axes, averaged over time interval and
1684 * expressed in radians per second (rad/s).
1685 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
1686 * @param result instance where new estimated NED frame containing new body position,
1687 * velocity and coordinate transformation matrix will be stored.
1688 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
1689 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
1690 * body-to-NED-frame coordinate transformation matrix are
1691 * invalid.
1692 */
1693 public void navigate(
1694 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
1695 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
1696 final double fx, final double fy, final double fz,
1697 final double angularRateX, final double angularRateY, final double angularRateZ,
1698 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
1699 InvalidSourceAndDestinationFrameTypeException {
1700 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
1701 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
1702 }
1703
1704 /**
1705 * Runs precision local-navigation-frame inertial navigation equations.
1706 * NOTE: only the attitude update and specific force frame transformation
1707 * phases are precise.
1708 *
1709 * @param timeInterval time interval between epochs.
1710 * @param oldLatitude previous latitude angle.
1711 * @param oldLongitude previous longitude angle.
1712 * @param oldHeight previous height.
1713 * @param oldC previous body-to-NED coordinate transformation.
1714 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
1715 * along north, east and down axes.
1716 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
1717 * resolved along body-frame axes, averaged over time interval and
1718 * expressed in meters per squared second (m/s^2).
1719 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
1720 * resolved along body-frame axes, averaged over time interval and
1721 * expressed in meters per squared second (m/s^2).
1722 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
1723 * resolved along body-frame axes, averaged over time interval and
1724 * expressed in meters per squared second (m/s^2).
1725 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
1726 * resolved along body-frame axes, averaged over time interval and
1727 * expressed in radians per second (rad/s).
1728 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
1729 * resolved along body-frame axes, averaged over time interval and
1730 * expressed in radians per second (rad/s).
1731 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
1732 * resolved along body-frame axes, averaged over time interval and
1733 * expressed in radians per second (rad/s).
1734 * @param result instance where new estimated NED frame containing new body position,
1735 * velocity and coordinate transformation matrix will be stored.
1736 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
1737 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
1738 * body-to-NED-frame coordinate transformation matrix are
1739 * invalid.
1740 */
1741 public void navigate(
1742 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
1743 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
1744 final double fx, final double fy, final double fz,
1745 final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result)
1746 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
1747 navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
1748 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
1749 }
1750
1751 /**
1752 * Runs precision local-navigation-frame inertial navigation equations.
1753 * NOTE: only the attitude update and specific force frame transformation
1754 * phases are precise.
1755 *
1756 * @param timeInterval time interval between epochs expressed in seconds (s).
1757 * @param oldLatitude previous latitude angle.
1758 * @param oldLongitude previous longitude angle.
1759 * @param oldHeight previous height.
1760 * @param oldC previous body-to-NED coordinate transformation.
1761 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
1762 * resolved along NED-frame axes and expressed in meters per second (m/s).
1763 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
1764 * resolved along NED-frame axes and expressed in meters per second (m/s).
1765 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
1766 * resolved along NED-frame axes and expressed in meters per second (m/s).
1767 * @param kinematics body kinematics containing specific forces and angular rates applied to
1768 * the body.
1769 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
1770 * @param result instance where new estimated NED frame containing new body position,
1771 * velocity and coordinate transformation matrix will be stored.
1772 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
1773 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
1774 * body-to-NED-frame coordinate transformation matrix are
1775 * invalid.
1776 */
1777 public void navigate(
1778 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
1779 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
1780 final BodyKinematics kinematics, final double accuracyThreshold, final NEDFrame result)
1781 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
1782 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, kinematics,
1783 accuracyThreshold, result);
1784 }
1785
1786 /**
1787 * Runs precision local-navigation-frame inertial navigation equations.
1788 * NOTE: only the attitude update and specific force frame transformation
1789 * phases are precise.
1790 *
1791 * @param timeInterval time interval between epochs expressed in seconds (s).
1792 * @param oldLatitude previous latitude angle.
1793 * @param oldLongitude previous longitude angle.
1794 * @param oldHeight previous height.
1795 * @param oldC previous body-to-NED coordinate transformation.
1796 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
1797 * resolved along NED-frame axes and expressed in meters per second (m/s).
1798 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
1799 * resolved along NED-frame axes and expressed in meters per second (m/s).
1800 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
1801 * resolved along NED-frame axes and expressed in meters per second (m/s).
1802 * @param kinematics body kinematics containing specific forces and angular rates applied to
1803 * the body.
1804 * @param result instance where new estimated NED frame containing new body position,
1805 * velocity and coordinate transformation matrix will be stored.
1806 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
1807 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
1808 * body-to-NED-frame coordinate transformation matrix are
1809 * invalid.
1810 */
1811 public void navigate(
1812 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
1813 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
1814 final BodyKinematics kinematics, final NEDFrame result) throws InertialNavigatorException,
1815 InvalidSourceAndDestinationFrameTypeException {
1816 navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, kinematics,
1817 DEFAULT_ACCURACY_THRESHOLD, result);
1818 }
1819
1820 /**
1821 * Runs precision local-navigation-frame inertial navigation equations.
1822 * NOTE: only the attitude update and specific force frame transformation
1823 * phases are precise.
1824 *
1825 * @param timeInterval time interval between epochs.
1826 * @param oldLatitude previous latitude angle.
1827 * @param oldLongitude previous longitude angle.
1828 * @param oldHeight previous height.
1829 * @param oldC previous body-to-NED coordinate transformation.
1830 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
1831 * resolved along NED-frame axes and expressed in meters per second (m/s).
1832 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
1833 * resolved along NED-frame axes and expressed in meters per second (m/s).
1834 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
1835 * resolved along NED-frame axes and expressed in meters per second (m/s).
1836 * @param kinematics body kinematics containing specific forces and angular rates applied to
1837 * the body.
1838 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
1839 * @param result instance where new estimated NED frame containing new body position,
1840 * velocity and coordinate transformation matrix will be stored.
1841 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
1842 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
1843 * body-to-NED-frame coordinate transformation matrix are
1844 * invalid.
1845 */
1846 public void navigate(
1847 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
1848 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
1849 final BodyKinematics kinematics, final double accuracyThreshold, final NEDFrame result)
1850 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
1851 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, kinematics,
1852 accuracyThreshold, result);
1853 }
1854
1855 /**
1856 * Runs precision local-navigation-frame inertial navigation equations.
1857 * NOTE: only the attitude update and specific force frame transformation
1858 * phases are precise.
1859 *
1860 * @param timeInterval time interval between epochs.
1861 * @param oldLatitude previous latitude angle.
1862 * @param oldLongitude previous longitude angle.
1863 * @param oldHeight previous height.
1864 * @param oldC previous body-to-NED coordinate transformation.
1865 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
1866 * resolved along NED-frame axes and expressed in meters per second (m/s).
1867 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
1868 * resolved along NED-frame axes and expressed in meters per second (m/s).
1869 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
1870 * resolved along NED-frame axes and expressed in meters per second (m/s).
1871 * @param kinematics body kinematics containing specific forces and angular rates applied to
1872 * the body.
1873 * @param result instance where new estimated NED frame containing new body position,
1874 * velocity and coordinate transformation matrix will be stored.
1875 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
1876 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
1877 * body-to-NED-frame coordinate transformation matrix are
1878 * invalid.
1879 */
1880 public void navigate(
1881 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
1882 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
1883 final BodyKinematics kinematics, final NEDFrame result) throws InertialNavigatorException,
1884 InvalidSourceAndDestinationFrameTypeException {
1885 navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, kinematics,
1886 DEFAULT_ACCURACY_THRESHOLD, result);
1887 }
1888
1889 /**
1890 * Runs precision local-navigation-frame inertial navigation equations.
1891 * NOTE: only the attitude update and specific force frame transformation
1892 * phases are precise.
1893 *
1894 * @param timeInterval time interval between epochs expressed in seconds (s).
1895 * @param oldLatitude previous latitude angle.
1896 * @param oldLongitude previous longitude angle.
1897 * @param oldHeight previous height.
1898 * @param oldC previous body-to-NED coordinate transformation.
1899 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
1900 * along north, east and down axes.
1901 * @param kinematics body kinematics containing specific forces and angular rates applied to
1902 * the body.
1903 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
1904 * @param result instance where new estimated NED frame containing new body position,
1905 * velocity and coordinate transformation matrix will be stored.
1906 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
1907 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
1908 * body-to-NED-frame coordinate transformation matrix are
1909 * invalid.
1910 */
1911 public void navigate(
1912 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
1913 final CoordinateTransformation oldC, final NEDVelocity oldVelocity, final BodyKinematics kinematics,
1914 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
1915 InvalidSourceAndDestinationFrameTypeException {
1916 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, kinematics,
1917 accuracyThreshold, result);
1918 }
1919
1920 /**
1921 * Runs precision local-navigation-frame inertial navigation equations.
1922 * NOTE: only the attitude update and specific force frame transformation
1923 * phases are precise.
1924 *
1925 * @param timeInterval time interval between epochs expressed in seconds (s).
1926 * @param oldLatitude previous latitude angle.
1927 * @param oldLongitude previous longitude angle.
1928 * @param oldHeight previous height.
1929 * @param oldC previous body-to-NED coordinate transformation.
1930 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
1931 * along north, east and down axes.
1932 * @param kinematics body kinematics containing specific forces and angular rates applied to
1933 * the body.
1934 * @param result instance where new estimated NED frame containing new body position,
1935 * velocity and coordinate transformation matrix will be stored.
1936 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
1937 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
1938 * body-to-NED-frame coordinate transformation matrix are
1939 * invalid.
1940 */
1941 public void navigate(
1942 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
1943 final CoordinateTransformation oldC, final NEDVelocity oldVelocity, final BodyKinematics kinematics,
1944 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
1945 navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, kinematics,
1946 DEFAULT_ACCURACY_THRESHOLD, result);
1947 }
1948
1949 /**
1950 * Runs precision local-navigation-frame inertial navigation equations.
1951 * NOTE: only the attitude update and specific force frame transformation
1952 * phases are precise.
1953 *
1954 * @param timeInterval time interval between epochs.
1955 * @param oldLatitude previous latitude angle.
1956 * @param oldLongitude previous longitude angle.
1957 * @param oldHeight previous height.
1958 * @param oldC previous body-to-NED coordinate transformation.
1959 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
1960 * along north, east and down axes.
1961 * @param kinematics body kinematics containing specific forces and angular rates applied to
1962 * the body.
1963 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
1964 * @param result instance where new estimated NED frame containing new body position,
1965 * velocity and coordinate transformation matrix will be stored.
1966 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
1967 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
1968 * body-to-NED-frame coordinate transformation matrix are
1969 * invalid.
1970 */
1971 public void navigate(
1972 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
1973 final CoordinateTransformation oldC, final NEDVelocity oldVelocity, final BodyKinematics kinematics,
1974 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
1975 InvalidSourceAndDestinationFrameTypeException {
1976 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, kinematics,
1977 accuracyThreshold, result);
1978 }
1979
1980 /**
1981 * Runs precision local-navigation-frame inertial navigation equations.
1982 * NOTE: only the attitude update and specific force frame transformation
1983 * phases are precise.
1984 *
1985 * @param timeInterval time interval between epochs.
1986 * @param oldLatitude previous latitude angle.
1987 * @param oldLongitude previous longitude angle.
1988 * @param oldHeight previous height.
1989 * @param oldC previous body-to-NED coordinate transformation.
1990 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
1991 * along north, east and down axes.
1992 * @param kinematics body kinematics containing specific forces and angular rates applied to
1993 * the body.
1994 * @param result instance where new estimated NED frame containing new body position,
1995 * velocity and coordinate transformation matrix will be stored.
1996 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
1997 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
1998 * body-to-NED-frame coordinate transformation matrix are
1999 * invalid.
2000 */
2001 public void navigate(
2002 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
2003 final CoordinateTransformation oldC, final NEDVelocity oldVelocity, final BodyKinematics kinematics,
2004 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
2005 navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, kinematics,
2006 DEFAULT_ACCURACY_THRESHOLD, result);
2007 }
2008
2009 /**
2010 * Runs precision local-navigation-frame inertial navigation equations.
2011 * NOTE: only the attitude update and specific force frame transformation
2012 * phases are precise.
2013 *
2014 * @param timeInterval time interval between epochs expressed in seconds (s).
2015 * @param oldLatitude previous latitude expressed in radians (rad).
2016 * @param oldLongitude previous longitude expressed in radians (rad).
2017 * @param oldHeight previous height expressed in meters (m).
2018 * @param oldC previous body-to-NED coordinate transformation.
2019 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
2020 * resolved along NED-frame axes.
2021 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
2022 * resolved along NED-frame axes.
2023 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
2024 * resolved along NED-frame axes.
2025 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
2026 * resolved along body-frame axes, averaged over time interval and
2027 * expressed in meters per squared second (m/s^2).
2028 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
2029 * resolved along body-frame axes, averaged over time interval and
2030 * expressed in meters per squared second (m/s^2).
2031 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
2032 * resolved along body-frame axes, averaged over time interval and
2033 * expressed in meters per squared second (m/s^2).
2034 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
2035 * resolved along body-frame axes, averaged over time interval and
2036 * expressed in radians per second (rad/s).
2037 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
2038 * resolved along body-frame axes, averaged over time interval and
2039 * expressed in radians per second (rad/s).
2040 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
2041 * resolved along body-frame axes, averaged over time interval and
2042 * expressed in radians per second (rad/s).
2043 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
2044 * @param result instance where new estimated NED frame containing new body position,
2045 * velocity and coordinate transformation matrix will be stored.
2046 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
2047 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
2048 * body-to-NED-frame coordinate transformation matrix are
2049 * invalid.
2050 */
2051 public void navigate(
2052 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
2053 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
2054 final double fx, final double fy, final double fz,
2055 final double angularRateX, final double angularRateY, final double angularRateZ,
2056 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
2057 InvalidSourceAndDestinationFrameTypeException {
2058 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
2059 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
2060 }
2061
2062 /**
2063 * Runs precision local-navigation-frame inertial navigation equations.
2064 * NOTE: only the attitude update and specific force frame transformation
2065 * phases are precise.
2066 *
2067 * @param timeInterval time interval between epochs expressed in seconds (s).
2068 * @param oldLatitude previous latitude expressed in radians (rad).
2069 * @param oldLongitude previous longitude expressed in radians (rad).
2070 * @param oldHeight previous height expressed in meters (m).
2071 * @param oldC previous body-to-NED coordinate transformation.
2072 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
2073 * resolved along NED-frame axes.
2074 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
2075 * resolved along NED-frame axes.
2076 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
2077 * resolved along NED-frame axes.
2078 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
2079 * resolved along body-frame axes, averaged over time interval and
2080 * expressed in meters per squared second (m/s^2).
2081 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
2082 * resolved along body-frame axes, averaged over time interval and
2083 * expressed in meters per squared second (m/s^2).
2084 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
2085 * resolved along body-frame axes, averaged over time interval and
2086 * expressed in meters per squared second (m/s^2).
2087 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
2088 * resolved along body-frame axes, averaged over time interval and
2089 * expressed in radians per second (rad/s).
2090 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
2091 * resolved along body-frame axes, averaged over time interval and
2092 * expressed in radians per second (rad/s).
2093 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
2094 * resolved along body-frame axes, averaged over time interval and
2095 * expressed in radians per second (rad/s).
2096 * @param result instance where new estimated NED frame containing new body position,
2097 * velocity and coordinate transformation matrix will be stored.
2098 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
2099 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
2100 * body-to-NED-frame coordinate transformation matrix are
2101 * invalid.
2102 */
2103 public void navigate(
2104 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
2105 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
2106 final double fx, final double fy, final double fz,
2107 final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result)
2108 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
2109 navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
2110 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
2111 }
2112
2113 /**
2114 * Runs precision local-navigation-frame inertial navigation equations.
2115 * NOTE: only the attitude update and specific force frame transformation
2116 * phases are precise.
2117 *
2118 * @param timeInterval time interval between epochs.
2119 * @param oldLatitude previous latitude expressed in radians (rad).
2120 * @param oldLongitude previous longitude expressed in radians (rad).
2121 * @param oldHeight previous height expressed in meters (m).
2122 * @param oldC previous body-to-NED coordinate transformation.
2123 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
2124 * resolved along NED-frame axes.
2125 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
2126 * resolved along NED-frame axes.
2127 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
2128 * resolved along NED-frame axes.
2129 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
2130 * resolved along body-frame axes, averaged over time interval and
2131 * expressed in meters per squared second (m/s^2).
2132 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
2133 * resolved along body-frame axes, averaged over time interval and
2134 * expressed in meters per squared second (m/s^2).
2135 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
2136 * resolved along body-frame axes, averaged over time interval and
2137 * expressed in meters per squared second (m/s^2).
2138 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
2139 * resolved along body-frame axes, averaged over time interval and
2140 * expressed in radians per second (rad/s).
2141 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
2142 * resolved along body-frame axes, averaged over time interval and
2143 * expressed in radians per second (rad/s).
2144 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
2145 * resolved along body-frame axes, averaged over time interval and
2146 * expressed in radians per second (rad/s).
2147 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
2148 * @param result instance where new estimated NED frame containing new body position,
2149 * velocity and coordinate transformation matrix will be stored.
2150 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
2151 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
2152 * body-to-NED-frame coordinate transformation matrix are
2153 * invalid.
2154 */
2155 public void navigate(
2156 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
2157 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
2158 final double fx, final double fy, final double fz,
2159 final double angularRateX, final double angularRateY, final double angularRateZ,
2160 final double accuracyThreshold, final NEDFrame result)
2161 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
2162 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
2163 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
2164 }
2165
2166 /**
2167 * Runs precision local-navigation-frame inertial navigation equations.
2168 * NOTE: only the attitude update and specific force frame transformation
2169 * phases are precise.
2170 *
2171 * @param timeInterval time interval between epochs.
2172 * @param oldLatitude previous latitude expressed in radians (rad).
2173 * @param oldLongitude previous longitude expressed in radians (rad).
2174 * @param oldHeight previous height expressed in meters (m).
2175 * @param oldC previous body-to-NED coordinate transformation.
2176 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
2177 * resolved along NED-frame axes.
2178 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
2179 * resolved along NED-frame axes.
2180 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
2181 * resolved along NED-frame axes.
2182 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
2183 * resolved along body-frame axes, averaged over time interval and
2184 * expressed in meters per squared second (m/s^2).
2185 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
2186 * resolved along body-frame axes, averaged over time interval and
2187 * expressed in meters per squared second (m/s^2).
2188 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
2189 * resolved along body-frame axes, averaged over time interval and
2190 * expressed in meters per squared second (m/s^2).
2191 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
2192 * resolved along body-frame axes, averaged over time interval and
2193 * expressed in radians per second (rad/s).
2194 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
2195 * resolved along body-frame axes, averaged over time interval and
2196 * expressed in radians per second (rad/s).
2197 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
2198 * resolved along body-frame axes, averaged over time interval and
2199 * expressed in radians per second (rad/s).
2200 * @param result instance where new estimated NED frame containing new body position,
2201 * velocity and coordinate transformation matrix will be stored.
2202 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
2203 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
2204 * body-to-NED-frame coordinate transformation matrix are
2205 * invalid.
2206 */
2207 public void navigate(
2208 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
2209 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
2210 final double fx, final double fy, final double fz,
2211 final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result)
2212 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
2213 navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
2214 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
2215 }
2216
2217 /**
2218 * Runs precision local-navigation-frame inertial navigation equations.
2219 * NOTE: only the attitude update and specific force frame transformation
2220 * phases are precise.
2221 *
2222 * @param timeInterval time interval between epochs expressed in seconds (s).
2223 * @param oldPosition previous curvilinear position expressed in terms of latitude,
2224 * longitude and height.
2225 * @param oldC previous body-to-NED coordinate transformation.
2226 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
2227 * resolved along NED-frame axes.
2228 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
2229 * resolved along NED-frame axes.
2230 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
2231 * resolved along NED-frame axes.
2232 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
2233 * resolved along body-frame axes, averaged over time interval and
2234 * expressed in meters per squared second (m/s^2).
2235 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
2236 * resolved along body-frame axes, averaged over time interval and
2237 * expressed in meters per squared second (m/s^2).
2238 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
2239 * resolved along body-frame axes, averaged over time interval and
2240 * expressed in meters per squared second (m/s^2).
2241 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
2242 * resolved along body-frame axes, averaged over time interval and
2243 * expressed in radians per second (rad/s).
2244 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
2245 * resolved along body-frame axes, averaged over time interval and
2246 * expressed in radians per second (rad/s).
2247 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
2248 * resolved along body-frame axes, averaged over time interval and
2249 * expressed in radians per second (rad/s).
2250 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
2251 * @param result instance where new estimated NED frame containing new body position,
2252 * velocity and coordinate transformation matrix will be stored.
2253 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
2254 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
2255 * body-to-NED-frame coordinate transformation matrix are
2256 * invalid.
2257 */
2258 public void navigate(
2259 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
2260 final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
2261 final double fx, final double fy, final double fz,
2262 final double angularRateX, final double angularRateY, final double angularRateZ,
2263 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
2264 InvalidSourceAndDestinationFrameTypeException {
2265 navigateNED(timeInterval, oldPosition, oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
2266 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
2267 }
2268
2269 /**
2270 * Runs precision local-navigation-frame inertial navigation equations.
2271 * NOTE: only the attitude update and specific force frame transformation
2272 * phases are precise.
2273 *
2274 * @param timeInterval time interval between epochs expressed in seconds (s).
2275 * @param oldPosition previous curvilinear position expressed in terms of latitude,
2276 * longitude and height.
2277 * @param oldC previous body-to-NED coordinate transformation.
2278 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
2279 * resolved along NED-frame axes.
2280 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
2281 * resolved along NED-frame axes.
2282 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
2283 * resolved along NED-frame axes.
2284 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
2285 * resolved along body-frame axes, averaged over time interval and
2286 * expressed in meters per squared second (m/s^2).
2287 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
2288 * resolved along body-frame axes, averaged over time interval and
2289 * expressed in meters per squared second (m/s^2).
2290 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
2291 * resolved along body-frame axes, averaged over time interval and
2292 * expressed in meters per squared second (m/s^2).
2293 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
2294 * resolved along body-frame axes, averaged over time interval and
2295 * expressed in radians per second (rad/s).
2296 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
2297 * resolved along body-frame axes, averaged over time interval and
2298 * expressed in radians per second (rad/s).
2299 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
2300 * resolved along body-frame axes, averaged over time interval and
2301 * expressed in radians per second (rad/s).
2302 * @param result instance where new estimated NED frame containing new body position,
2303 * velocity and coordinate transformation matrix will be stored.
2304 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
2305 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
2306 * body-to-NED-frame coordinate transformation matrix are
2307 * invalid.
2308 */
2309 public void navigate(
2310 final double timeInterval, final NEDPosition oldPosition,
2311 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
2312 final double fx, final double fy, final double fz,
2313 final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result)
2314 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
2315 navigate(timeInterval, oldPosition, oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
2316 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
2317 }
2318
2319 /**
2320 * Runs precision local-navigation-frame inertial navigation equations.
2321 * NOTE: only the attitude update and specific force frame transformation
2322 * phases are precise.
2323 *
2324 * @param timeInterval time interval between epochs.
2325 * @param oldPosition previous curvilinear position expressed in terms of latitude,
2326 * longitude and height.
2327 * @param oldC previous body-to-NED coordinate transformation.
2328 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
2329 * resolved along NED-frame axes.
2330 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
2331 * resolved along NED-frame axes.
2332 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
2333 * resolved along NED-frame axes.
2334 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
2335 * resolved along body-frame axes, averaged over time interval and
2336 * expressed in meters per squared second (m/s^2).
2337 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
2338 * resolved along body-frame axes, averaged over time interval and
2339 * expressed in meters per squared second (m/s^2).
2340 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
2341 * resolved along body-frame axes, averaged over time interval and
2342 * expressed in meters per squared second (m/s^2).
2343 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
2344 * resolved along body-frame axes, averaged over time interval and
2345 * expressed in radians per second (rad/s).
2346 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
2347 * resolved along body-frame axes, averaged over time interval and
2348 * expressed in radians per second (rad/s).
2349 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
2350 * resolved along body-frame axes, averaged over time interval and
2351 * expressed in radians per second (rad/s).
2352 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
2353 * @param result instance where new estimated NED frame containing new body position,
2354 * velocity and coordinate transformation matrix will be stored.
2355 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
2356 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
2357 * body-to-NED-frame coordinate transformation matrix are
2358 * invalid.
2359 */
2360 public void navigate(
2361 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
2362 final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
2363 final double fx, final double fy, final double fz,
2364 final double angularRateX, final double angularRateY, final double angularRateZ,
2365 final double accuracyThreshold, final NEDFrame result)
2366 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
2367 navigateNED(timeInterval, oldPosition, oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
2368 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
2369 }
2370
2371 /**
2372 * Runs precision local-navigation-frame inertial navigation equations.
2373 * NOTE: only the attitude update and specific force frame transformation
2374 * phases are precise.
2375 *
2376 * @param timeInterval time interval between epochs.
2377 * @param oldPosition previous curvilinear position expressed in terms of latitude,
2378 * longitude and height.
2379 * @param oldC previous body-to-NED coordinate transformation.
2380 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
2381 * resolved along NED-frame axes.
2382 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
2383 * resolved along NED-frame axes.
2384 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
2385 * resolved along NED-frame axes.
2386 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
2387 * resolved along body-frame axes, averaged over time interval and
2388 * expressed in meters per squared second (m/s^2).
2389 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
2390 * resolved along body-frame axes, averaged over time interval and
2391 * expressed in meters per squared second (m/s^2).
2392 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
2393 * resolved along body-frame axes, averaged over time interval and
2394 * expressed in meters per squared second (m/s^2).
2395 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
2396 * resolved along body-frame axes, averaged over time interval and
2397 * expressed in radians per second (rad/s).
2398 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
2399 * resolved along body-frame axes, averaged over time interval and
2400 * expressed in radians per second (rad/s).
2401 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
2402 * resolved along body-frame axes, averaged over time interval and
2403 * expressed in radians per second (rad/s).
2404 * @param result instance where new estimated NED frame containing new body position,
2405 * velocity and coordinate transformation matrix will be stored.
2406 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
2407 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
2408 * body-to-NED-frame coordinate transformation matrix are
2409 * invalid.
2410 */
2411 public void navigate(
2412 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
2413 final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
2414 final double fx, final double fy, final double fz,
2415 final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result)
2416 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
2417 navigateNED(timeInterval, oldPosition, oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
2418 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
2419 }
2420
2421 /**
2422 * Runs precision local-navigation-frame inertial navigation equations.
2423 * NOTE: only the attitude update and specific force frame transformation
2424 * phases are precise.
2425 *
2426 * @param timeInterval time interval between epochs expressed in seconds (s).
2427 * @param oldLatitude previous latitude expressed in radians (rad).
2428 * @param oldLongitude previous longitude expressed in radians (rad).
2429 * @param oldHeight previous height expressed in meters (m).
2430 * @param oldC previous body-to-NED coordinate transformation.
2431 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
2432 * resolved along NED-frame axes.
2433 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
2434 * resolved along NED-frame axes.
2435 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
2436 * resolved along NED-frame axes.
2437 * @param kinematics body kinematics containing specific forces and angular rates applied to
2438 * the body.
2439 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
2440 * @param result instance where new estimated NED frame containing new body position,
2441 * velocity and coordinate transformation matrix will be stored.
2442 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
2443 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
2444 * body-to-NED-frame coordinate transformation matrix are
2445 * invalid.
2446 */
2447 public void navigate(
2448 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
2449 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
2450 final BodyKinematics kinematics, final double accuracyThreshold, final NEDFrame result)
2451 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
2452 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
2453 kinematics, accuracyThreshold, result);
2454 }
2455
2456 /**
2457 * Runs precision local-navigation-frame inertial navigation equations.
2458 * NOTE: only the attitude update and specific force frame transformation
2459 * phases are precise.
2460 *
2461 * @param timeInterval time interval between epochs expressed in seconds (s).
2462 * @param oldLatitude previous latitude expressed in radians (rad).
2463 * @param oldLongitude previous longitude expressed in radians (rad).
2464 * @param oldHeight previous height expressed in meters (m).
2465 * @param oldC previous body-to-NED coordinate transformation.
2466 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
2467 * resolved along NED-frame axes.
2468 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
2469 * resolved along NED-frame axes.
2470 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
2471 * resolved along NED-frame axes.
2472 * @param kinematics body kinematics containing specific forces and angular rates applied to
2473 * the body.
2474 * @param result instance where new estimated NED frame containing new body position,
2475 * velocity and coordinate transformation matrix will be stored.
2476 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
2477 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
2478 * body-to-NED-frame coordinate transformation matrix are
2479 * invalid.
2480 */
2481 public void navigate(
2482 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
2483 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
2484 final BodyKinematics kinematics, final NEDFrame result) throws InertialNavigatorException,
2485 InvalidSourceAndDestinationFrameTypeException {
2486 navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD, kinematics,
2487 DEFAULT_ACCURACY_THRESHOLD, result);
2488 }
2489
2490 /**
2491 * Runs precision local-navigation-frame inertial navigation equations.
2492 * NOTE: only the attitude update and specific force frame transformation
2493 * phases are precise.
2494 *
2495 * @param timeInterval time interval between epochs.
2496 * @param oldLatitude previous latitude expressed in radians (rad).
2497 * @param oldLongitude previous longitude expressed in radians (rad).
2498 * @param oldHeight previous height expressed in meters (m).
2499 * @param oldC previous body-to-NED coordinate transformation.
2500 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
2501 * resolved along NED-frame axes.
2502 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
2503 * resolved along NED-frame axes.
2504 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
2505 * resolved along NED-frame axes.
2506 * @param kinematics body kinematics containing specific forces and angular rates applied to
2507 * the body.
2508 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
2509 * @param result instance where new estimated NED frame containing new body position,
2510 * velocity and coordinate transformation matrix will be stored.
2511 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
2512 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
2513 * body-to-NED-frame coordinate transformation matrix are
2514 * invalid.
2515 */
2516 public void navigate(
2517 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
2518 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
2519 final BodyKinematics kinematics, final double accuracyThreshold, final NEDFrame result)
2520 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
2521 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
2522 kinematics, accuracyThreshold, result);
2523 }
2524
2525 /**
2526 * Runs precision local-navigation-frame inertial navigation equations.
2527 * NOTE: only the attitude update and specific force frame transformation
2528 * phases are precise.
2529 *
2530 * @param timeInterval time interval between epochs.
2531 * @param oldLatitude previous latitude expressed in radians (rad).
2532 * @param oldLongitude previous longitude expressed in radians (rad).
2533 * @param oldHeight previous height expressed in meters (m).
2534 * @param oldC previous body-to-NED coordinate transformation.
2535 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
2536 * resolved along NED-frame axes.
2537 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
2538 * resolved along NED-frame axes.
2539 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
2540 * resolved along NED-frame axes.
2541 * @param kinematics body kinematics containing specific forces and angular rates applied to
2542 * the body.
2543 * @param result instance where new estimated NED frame containing new body position,
2544 * velocity and coordinate transformation matrix will be stored.
2545 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
2546 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
2547 * body-to-NED-frame coordinate transformation matrix are
2548 * invalid.
2549 */
2550 public void navigate(
2551 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
2552 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
2553 final BodyKinematics kinematics, final NEDFrame result) throws InertialNavigatorException,
2554 InvalidSourceAndDestinationFrameTypeException {
2555 navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD, kinematics,
2556 DEFAULT_ACCURACY_THRESHOLD, result);
2557 }
2558
2559 /**
2560 * Runs precision local-navigation-frame inertial navigation equations.
2561 * NOTE: only the attitude update and specific force frame transformation
2562 * phases are precise.
2563 *
2564 * @param timeInterval time interval between epochs expressed in seconds (s).
2565 * @param oldPosition previous curvilinear position expressed in terms of latitude,
2566 * longitude and height.
2567 * @param oldC previous body-to-NED coordinate transformation.
2568 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
2569 * resolved along NED-frame axes.
2570 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
2571 * resolved along NED-frame axes.
2572 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
2573 * resolved along NED-frame axes.
2574 * @param kinematics body kinematics containing specific forces and angular rates applied to
2575 * the body.
2576 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
2577 * @param result instance where new estimated NED frame containing new body position,
2578 * velocity and coordinate transformation matrix will be stored.
2579 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
2580 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
2581 * body-to-NED-frame coordinate transformation matrix are
2582 * invalid.
2583 */
2584 public void navigate(
2585 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
2586 final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD, final BodyKinematics kinematics,
2587 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
2588 InvalidSourceAndDestinationFrameTypeException {
2589 navigateNED(timeInterval, oldPosition, oldC, oldSpeedN, oldSpeedE, oldSpeedD, kinematics, accuracyThreshold,
2590 result);
2591 }
2592
2593 /**
2594 * Runs precision local-navigation-frame inertial navigation equations.
2595 * NOTE: only the attitude update and specific force frame transformation
2596 * phases are precise.
2597 *
2598 * @param timeInterval time interval between epochs expressed in seconds (s).
2599 * @param oldPosition previous curvilinear position expressed in terms of latitude,
2600 * longitude and height.
2601 * @param oldC previous body-to-NED coordinate transformation.
2602 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
2603 * resolved along NED-frame axes.
2604 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
2605 * resolved along NED-frame axes.
2606 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
2607 * resolved along NED-frame axes.
2608 * @param kinematics body kinematics containing specific forces and angular rates applied to
2609 * the body.
2610 * @param result instance where new estimated NED frame containing new body position,
2611 * velocity and coordinate transformation matrix will be stored.
2612 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
2613 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
2614 * body-to-NED-frame coordinate transformation matrix are
2615 * invalid.
2616 */
2617 public void navigate(
2618 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
2619 final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD, final BodyKinematics kinematics,
2620 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
2621 navigate(timeInterval, oldPosition, oldC, oldSpeedN, oldSpeedE, oldSpeedD, kinematics,
2622 DEFAULT_ACCURACY_THRESHOLD, result);
2623 }
2624
2625 /**
2626 * Runs precision local-navigation-frame inertial navigation equations.
2627 * NOTE: only the attitude update and specific force frame transformation
2628 * phases are precise.
2629 *
2630 * @param timeInterval time interval between epochs.
2631 * @param oldPosition previous curvilinear position expressed in terms of latitude,
2632 * longitude and height.
2633 * @param oldC previous body-to-NED coordinate transformation.
2634 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
2635 * resolved along NED-frame axes.
2636 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
2637 * resolved along NED-frame axes.
2638 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
2639 * resolved along NED-frame axes.
2640 * @param kinematics body kinematics containing specific forces and angular rates applied to
2641 * the body.
2642 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
2643 * @param result instance where new estimated NED frame containing new body position,
2644 * velocity and coordinate transformation matrix will be stored.
2645 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
2646 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
2647 * body-to-NED-frame coordinate transformation matrix are
2648 * invalid.
2649 */
2650 public void navigate(
2651 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
2652 final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD, final BodyKinematics kinematics,
2653 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
2654 InvalidSourceAndDestinationFrameTypeException {
2655 navigateNED(timeInterval, oldPosition, oldC, oldSpeedN, oldSpeedE, oldSpeedD, kinematics, accuracyThreshold,
2656 result);
2657 }
2658
2659 /**
2660 * Runs precision local-navigation-frame inertial navigation equations.
2661 * NOTE: only the attitude update and specific force frame transformation
2662 * phases are precise.
2663 *
2664 * @param timeInterval time interval between epochs.
2665 * @param oldPosition previous curvilinear position expressed in terms of latitude,
2666 * longitude and height.
2667 * @param oldC previous body-to-NED coordinate transformation.
2668 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
2669 * resolved along NED-frame axes.
2670 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
2671 * resolved along NED-frame axes.
2672 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
2673 * resolved along NED-frame axes.
2674 * @param kinematics body kinematics containing specific forces and angular rates applied to
2675 * the body.
2676 * @param result instance where new estimated NED frame containing new body position,
2677 * velocity and coordinate transformation matrix will be stored.
2678 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
2679 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
2680 * body-to-NED-frame coordinate transformation matrix are
2681 * invalid.
2682 */
2683 public void navigate(
2684 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
2685 final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD, final BodyKinematics kinematics,
2686 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
2687 navigate(timeInterval, oldPosition, oldC, oldSpeedN, oldSpeedE, oldSpeedD, kinematics,
2688 DEFAULT_ACCURACY_THRESHOLD, result);
2689 }
2690
2691 /**
2692 * Runs precision local-navigation-frame inertial navigation equations.
2693 * NOTE: only the attitude update and specific force frame transformation
2694 * phases are precise.
2695 *
2696 * @param timeInterval time interval between epochs expressed in seconds (s).
2697 * @param oldLatitude previous latitude expressed in radians (rad).
2698 * @param oldLongitude previous longitude expressed in radians (rad).
2699 * @param oldHeight previous height expressed in meters (m).
2700 * @param oldC previous body-to-NED coordinate transformation.
2701 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
2702 * resolved along NED-frame axes and expressed in meters per second (m/s).
2703 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
2704 * resolved along NED-frame axes and expressed in meters per second (m/s).
2705 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
2706 * resolved along NED-frame axes and expressed in meters per second (m/s).
2707 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
2708 * resolved along body-frame axes, averaged over time interval.
2709 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
2710 * resolved along body-frame axes, averaged over time interval.
2711 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
2712 * resolved along body-frame axes, averaged over time interval.
2713 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
2714 * resolved along body-frame axes, averaged over time interval and
2715 * expressed in radians per second (rad/s).
2716 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
2717 * resolved along body-frame axes, averaged over time interval and
2718 * expressed in radians per second (rad/s).
2719 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
2720 * resolved along body-frame axes, averaged over time interval and
2721 * expressed in radians per second (rad/s).
2722 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
2723 * @param result instance where new estimated NED frame containing new body position,
2724 * velocity and coordinate transformation matrix will be stored.
2725 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
2726 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
2727 * body-to-NED-frame coordinate transformation matrix are
2728 * invalid.
2729 */
2730 public void navigate(
2731 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
2732 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
2733 final Acceleration fx, final Acceleration fy, final Acceleration fz,
2734 final double angularRateX, final double angularRateY, final double angularRateZ,
2735 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
2736 InvalidSourceAndDestinationFrameTypeException {
2737 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
2738 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
2739 }
2740
2741 /**
2742 * Runs precision local-navigation-frame inertial navigation equations.
2743 * NOTE: only the attitude update and specific force frame transformation
2744 * phases are precise.
2745 *
2746 * @param timeInterval time interval between epochs expressed in seconds (s).
2747 * @param oldLatitude previous latitude expressed in radians (rad).
2748 * @param oldLongitude previous longitude expressed in radians (rad).
2749 * @param oldHeight previous height expressed in meters (m).
2750 * @param oldC previous body-to-NED coordinate transformation.
2751 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
2752 * resolved along NED-frame axes and expressed in meters per second (m/s).
2753 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
2754 * resolved along NED-frame axes and expressed in meters per second (m/s).
2755 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
2756 * resolved along NED-frame axes and expressed in meters per second (m/s).
2757 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
2758 * resolved along body-frame axes, averaged over time interval.
2759 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
2760 * resolved along body-frame axes, averaged over time interval.
2761 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
2762 * resolved along body-frame axes, averaged over time interval.
2763 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
2764 * resolved along body-frame axes, averaged over time interval and
2765 * expressed in radians per second (rad/s).
2766 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
2767 * resolved along body-frame axes, averaged over time interval and
2768 * expressed in radians per second (rad/s).
2769 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
2770 * resolved along body-frame axes, averaged over time interval and
2771 * expressed in radians per second (rad/s).
2772 * @param result instance where new estimated NED frame containing new body position,
2773 * velocity and coordinate transformation matrix will be stored.
2774 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
2775 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
2776 * body-to-NED-frame coordinate transformation matrix are
2777 * invalid.
2778 */
2779 public void navigate(
2780 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
2781 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
2782 final Acceleration fx, final Acceleration fy, final Acceleration fz,
2783 final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result)
2784 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
2785 navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
2786 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
2787 }
2788
2789 /**
2790 * Runs precision local-navigation-frame inertial navigation equations.
2791 * NOTE: only the attitude update and specific force frame transformation
2792 * phases are precise.
2793 *
2794 * @param timeInterval time interval between epochs.
2795 * @param oldLatitude previous latitude expressed in radians (rad).
2796 * @param oldLongitude previous longitude expressed in radians (rad).
2797 * @param oldHeight previous height expressed in meters (m).
2798 * @param oldC previous body-to-NED coordinate transformation.
2799 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
2800 * resolved along NED-frame axes and expressed in meters per second (m/s).
2801 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
2802 * resolved along NED-frame axes and expressed in meters per second (m/s).
2803 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
2804 * resolved along NED-frame axes and expressed in meters per second (m/s).
2805 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
2806 * resolved along body-frame axes, averaged over time interval.
2807 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
2808 * resolved along body-frame axes, averaged over time interval.
2809 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
2810 * resolved along body-frame axes, averaged over time interval.
2811 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
2812 * resolved along body-frame axes, averaged over time interval and
2813 * expressed in radians per second (rad/s).
2814 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
2815 * resolved along body-frame axes, averaged over time interval and
2816 * expressed in radians per second (rad/s).
2817 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
2818 * resolved along body-frame axes, averaged over time interval and
2819 * expressed in radians per second (rad/s).
2820 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
2821 * @param result instance where new estimated NED frame containing new body position,
2822 * velocity and coordinate transformation matrix will be stored.
2823 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
2824 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
2825 * body-to-NED-frame coordinate transformation matrix are
2826 * invalid.
2827 */
2828 public void navigate(
2829 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
2830 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
2831 final Acceleration fx, final Acceleration fy, final Acceleration fz,
2832 final double angularRateX, final double angularRateY, final double angularRateZ,
2833 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
2834 InvalidSourceAndDestinationFrameTypeException {
2835 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
2836 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
2837 }
2838
2839 /**
2840 * Runs precision local-navigation-frame inertial navigation equations.
2841 * NOTE: only the attitude update and specific force frame transformation
2842 * phases are precise.
2843 *
2844 * @param timeInterval time interval between epochs.
2845 * @param oldLatitude previous latitude expressed in radians (rad).
2846 * @param oldLongitude previous longitude expressed in radians (rad).
2847 * @param oldHeight previous height expressed in meters (m).
2848 * @param oldC previous body-to-NED coordinate transformation.
2849 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
2850 * resolved along NED-frame axes and expressed in meters per second (m/s).
2851 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
2852 * resolved along NED-frame axes and expressed in meters per second (m/s).
2853 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
2854 * resolved along NED-frame axes and expressed in meters per second (m/s).
2855 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
2856 * resolved along body-frame axes, averaged over time interval.
2857 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
2858 * resolved along body-frame axes, averaged over time interval.
2859 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
2860 * resolved along body-frame axes, averaged over time interval.
2861 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
2862 * resolved along body-frame axes, averaged over time interval and
2863 * expressed in radians per second (rad/s).
2864 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
2865 * resolved along body-frame axes, averaged over time interval and
2866 * expressed in radians per second (rad/s).
2867 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
2868 * resolved along body-frame axes, averaged over time interval and
2869 * expressed in radians per second (rad/s).
2870 * @param result instance where new estimated NED frame containing new body position,
2871 * velocity and coordinate transformation matrix will be stored.
2872 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
2873 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
2874 * body-to-NED-frame coordinate transformation matrix are
2875 * invalid.
2876 */
2877 public void navigate(
2878 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
2879 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
2880 final Acceleration fx, final Acceleration fy, final Acceleration fz,
2881 final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result)
2882 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
2883 navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
2884 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
2885 }
2886
2887 /**
2888 * Runs precision local-navigation-frame inertial navigation equations.
2889 * NOTE: only the attitude update and specific force frame transformation
2890 * phases are precise.
2891 *
2892 * @param timeInterval time interval between epochs expressed in seconds (s).
2893 * @param oldPosition previous curvilinear position expressed in terms of latitude,
2894 * longitude and height.
2895 * @param oldC previous body-to-NED coordinate transformation.
2896 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
2897 * resolved along NED-frame axes and expressed in meters per second (m/s).
2898 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
2899 * resolved along NED-frame axes and expressed in meters per second (m/s).
2900 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
2901 * resolved along NED-frame axes and expressed in meters per second (m/s).
2902 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
2903 * resolved along body-frame axes, averaged over time interval.
2904 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
2905 * resolved along body-frame axes, averaged over time interval.
2906 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
2907 * resolved along body-frame axes, averaged over time interval.
2908 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
2909 * resolved along body-frame axes, averaged over time interval and
2910 * expressed in radians per second (rad/s).
2911 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
2912 * resolved along body-frame axes, averaged over time interval and
2913 * expressed in radians per second (rad/s).
2914 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
2915 * resolved along body-frame axes, averaged over time interval and
2916 * expressed in radians per second (rad/s).
2917 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
2918 * @param result instance where new estimated NED frame containing new body position,
2919 * velocity and coordinate transformation matrix will be stored.
2920 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
2921 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
2922 * body-to-NED-frame coordinate transformation matrix are
2923 * invalid.
2924 */
2925 public void navigate(
2926 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
2927 final double oldVn, final double oldVe, final double oldVd,
2928 final Acceleration fx, final Acceleration fy, final Acceleration fz,
2929 final double angularRateX, final double angularRateY, final double angularRateZ,
2930 final double accuracyThreshold, final NEDFrame result)
2931 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
2932 navigateNED(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz, angularRateX,
2933 angularRateY, angularRateZ, accuracyThreshold, result);
2934 }
2935
2936 /**
2937 * Runs precision local-navigation-frame inertial navigation equations.
2938 * NOTE: only the attitude update and specific force frame transformation
2939 * phases are precise.
2940 *
2941 * @param timeInterval time interval between epochs expressed in seconds (s).
2942 * @param oldPosition previous curvilinear position expressed in terms of latitude,
2943 * longitude and height.
2944 * @param oldC previous body-to-NED coordinate transformation.
2945 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
2946 * resolved along NED-frame axes and expressed in meters per second (m/s).
2947 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
2948 * resolved along NED-frame axes and expressed in meters per second (m/s).
2949 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
2950 * resolved along NED-frame axes and expressed in meters per second (m/s).
2951 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
2952 * resolved along body-frame axes, averaged over time interval.
2953 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
2954 * resolved along body-frame axes, averaged over time interval.
2955 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
2956 * resolved along body-frame axes, averaged over time interval.
2957 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
2958 * resolved along body-frame axes, averaged over time interval and
2959 * expressed in radians per second (rad/s).
2960 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
2961 * resolved along body-frame axes, averaged over time interval and
2962 * expressed in radians per second (rad/s).
2963 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
2964 * resolved along body-frame axes, averaged over time interval and
2965 * expressed in radians per second (rad/s).
2966 * @param result instance where new estimated NED frame containing new body position,
2967 * velocity and coordinate transformation matrix will be stored.
2968 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
2969 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
2970 * body-to-NED-frame coordinate transformation matrix are
2971 * invalid.
2972 */
2973 public void navigate(
2974 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
2975 final double oldVn, final double oldVe, final double oldVd,
2976 final Acceleration fx, final Acceleration fy, final Acceleration fz,
2977 final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result)
2978 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
2979 navigate(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
2980 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
2981 }
2982
2983 /**
2984 * Runs precision local-navigation-frame inertial navigation equations.
2985 * NOTE: only the attitude update and specific force frame transformation
2986 * phases are precise.
2987 *
2988 * @param timeInterval time interval between epochs.
2989 * @param oldPosition previous curvilinear position expressed in terms of latitude,
2990 * longitude and height.
2991 * @param oldC previous body-to-NED coordinate transformation.
2992 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
2993 * resolved along NED-frame axes and expressed in meters per second (m/s).
2994 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
2995 * resolved along NED-frame axes and expressed in meters per second (m/s).
2996 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
2997 * resolved along NED-frame axes and expressed in meters per second (m/s).
2998 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
2999 * resolved along body-frame axes, averaged over time interval.
3000 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
3001 * resolved along body-frame axes, averaged over time interval.
3002 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
3003 * resolved along body-frame axes, averaged over time interval.
3004 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
3005 * resolved along body-frame axes, averaged over time interval and
3006 * expressed in radians per second (rad/s).
3007 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
3008 * resolved along body-frame axes, averaged over time interval and
3009 * expressed in radians per second (rad/s).
3010 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
3011 * resolved along body-frame axes, averaged over time interval and
3012 * expressed in radians per second (rad/s).
3013 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
3014 * @param result instance where new estimated NED frame containing new body position,
3015 * velocity and coordinate transformation matrix will be stored.
3016 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
3017 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
3018 * body-to-NED-frame coordinate transformation matrix are
3019 * invalid.
3020 */
3021 public void navigate(
3022 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
3023 final double oldVn, final double oldVe, final double oldVd,
3024 final Acceleration fx, final Acceleration fy, final Acceleration fz,
3025 final double angularRateX, final double angularRateY, final double angularRateZ,
3026 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
3027 InvalidSourceAndDestinationFrameTypeException {
3028 navigateNED(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
3029 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
3030 }
3031
3032 /**
3033 * Runs precision local-navigation-frame inertial navigation equations.
3034 * NOTE: only the attitude update and specific force frame transformation
3035 * phases are precise.
3036 *
3037 * @param timeInterval time interval between epochs.
3038 * @param oldPosition previous curvilinear position expressed in terms of latitude,
3039 * longitude and height.
3040 * @param oldC previous body-to-NED coordinate transformation.
3041 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
3042 * resolved along NED-frame axes and expressed in meters per second (m/s).
3043 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
3044 * resolved along NED-frame axes and expressed in meters per second (m/s).
3045 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
3046 * resolved along NED-frame axes and expressed in meters per second (m/s).
3047 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
3048 * resolved along body-frame axes, averaged over time interval.
3049 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
3050 * resolved along body-frame axes, averaged over time interval.
3051 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
3052 * resolved along body-frame axes, averaged over time interval.
3053 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
3054 * resolved along body-frame axes, averaged over time interval and
3055 * expressed in radians per second (rad/s).
3056 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
3057 * resolved along body-frame axes, averaged over time interval and
3058 * expressed in radians per second (rad/s).
3059 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
3060 * resolved along body-frame axes, averaged over time interval and
3061 * expressed in radians per second (rad/s).
3062 * @param result instance where new estimated NED frame containing new body position,
3063 * velocity and coordinate transformation matrix will be stored.
3064 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
3065 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
3066 * body-to-NED-frame coordinate transformation matrix are
3067 * invalid.
3068 */
3069 public void navigate(
3070 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
3071 final double oldVn, final double oldVe, final double oldVd,
3072 final Acceleration fx, final Acceleration fy, final Acceleration fz,
3073 final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result)
3074 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
3075 navigate(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
3076 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
3077 }
3078
3079 /**
3080 * Runs precision local-navigation-frame inertial navigation equations.
3081 * NOTE: only the attitude update and specific force frame transformation
3082 * phases are precise.
3083 *
3084 * @param timeInterval time interval between epochs expressed in seconds (s).
3085 * @param oldLatitude previous latitude expressed in radians (rad).
3086 * @param oldLongitude previous longitude expressed in radians (rad).
3087 * @param oldHeight previous height expressed in meters (m).
3088 * @param oldC previous body-to-NED coordinate transformation.
3089 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
3090 * along north, east and down axes.
3091 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
3092 * resolved along body-frame axes, averaged over time interval.
3093 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
3094 * resolved along body-frame axes, averaged over time interval.
3095 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
3096 * resolved along body-frame axes, averaged over time interval.
3097 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
3098 * resolved along body-frame axes, averaged over time interval and
3099 * expressed in radians per second (rad/s).
3100 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
3101 * resolved along body-frame axes, averaged over time interval and
3102 * expressed in radians per second (rad/s).
3103 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
3104 * resolved along body-frame axes, averaged over time interval and
3105 * expressed in radians per second (rad/s).
3106 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
3107 * @param result instance where new estimated NED frame containing new body position,
3108 * velocity and coordinate transformation matrix will be stored.
3109 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
3110 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
3111 * body-to-NED-frame coordinate transformation matrix are
3112 * invalid.
3113 */
3114 public void navigate(
3115 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
3116 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
3117 final Acceleration fx, final Acceleration fy, final Acceleration fz,
3118 final double angularRateX, final double angularRateY, final double angularRateZ,
3119 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
3120 InvalidSourceAndDestinationFrameTypeException {
3121 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
3122 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
3123 }
3124
3125 /**
3126 * Runs precision local-navigation-frame inertial navigation equations.
3127 * NOTE: only the attitude update and specific force frame transformation
3128 * phases are precise.
3129 *
3130 * @param timeInterval time interval between epochs expressed in seconds (s).
3131 * @param oldLatitude previous latitude expressed in radians (rad).
3132 * @param oldLongitude previous longitude expressed in radians (rad).
3133 * @param oldHeight previous height expressed in meters (m).
3134 * @param oldC previous body-to-NED coordinate transformation.
3135 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
3136 * along north, east and down axes.
3137 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
3138 * resolved along body-frame axes, averaged over time interval.
3139 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
3140 * resolved along body-frame axes, averaged over time interval.
3141 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
3142 * resolved along body-frame axes, averaged over time interval.
3143 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
3144 * resolved along body-frame axes, averaged over time interval and
3145 * expressed in radians per second (rad/s).
3146 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
3147 * resolved along body-frame axes, averaged over time interval and
3148 * expressed in radians per second (rad/s).
3149 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
3150 * resolved along body-frame axes, averaged over time interval and
3151 * expressed in radians per second (rad/s).
3152 * @param result instance where new estimated NED frame containing new body position,
3153 * velocity and coordinate transformation matrix will be stored.
3154 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
3155 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
3156 * body-to-NED-frame coordinate transformation matrix are
3157 * invalid.
3158 */
3159 public void navigate(
3160 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
3161 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
3162 final Acceleration fx, final Acceleration fy, final Acceleration fz,
3163 final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result)
3164 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
3165 navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
3166 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
3167 }
3168
3169 /**
3170 * Runs precision local-navigation-frame inertial navigation equations.
3171 * NOTE: only the attitude update and specific force frame transformation
3172 * phases are precise.
3173 *
3174 * @param timeInterval time interval between epochs.
3175 * @param oldLatitude previous latitude expressed in radians (rad).
3176 * @param oldLongitude previous longitude expressed in radians (rad).
3177 * @param oldHeight previous height expressed in meters (m).
3178 * @param oldC previous body-to-NED coordinate transformation.
3179 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
3180 * along north, east and down axes.
3181 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
3182 * resolved along body-frame axes, averaged over time interval.
3183 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
3184 * resolved along body-frame axes, averaged over time interval.
3185 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
3186 * resolved along body-frame axes, averaged over time interval.
3187 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
3188 * resolved along body-frame axes, averaged over time interval and
3189 * expressed in radians per second (rad/s).
3190 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
3191 * resolved along body-frame axes, averaged over time interval and
3192 * expressed in radians per second (rad/s).
3193 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
3194 * resolved along body-frame axes, averaged over time interval and
3195 * expressed in radians per second (rad/s).
3196 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
3197 * @param result instance where new estimated NED frame containing new body position,
3198 * velocity and coordinate transformation matrix will be stored.
3199 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
3200 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
3201 * body-to-NED-frame coordinate transformation matrix are
3202 * invalid.
3203 */
3204 public void navigate(
3205 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
3206 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
3207 final Acceleration fx, final Acceleration fy, final Acceleration fz,
3208 final double angularRateX, final double angularRateY, final double angularRateZ,
3209 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
3210 InvalidSourceAndDestinationFrameTypeException {
3211 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
3212 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
3213 }
3214
3215 /**
3216 * Runs precision local-navigation-frame inertial navigation equations.
3217 * NOTE: only the attitude update and specific force frame transformation
3218 * phases are precise.
3219 *
3220 * @param timeInterval time interval between epochs.
3221 * @param oldLatitude previous latitude expressed in radians (rad).
3222 * @param oldLongitude previous longitude expressed in radians (rad).
3223 * @param oldHeight previous height expressed in meters (m).
3224 * @param oldC previous body-to-NED coordinate transformation.
3225 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
3226 * along north, east and down axes.
3227 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
3228 * resolved along body-frame axes, averaged over time interval.
3229 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
3230 * resolved along body-frame axes, averaged over time interval.
3231 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
3232 * resolved along body-frame axes, averaged over time interval.
3233 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
3234 * resolved along body-frame axes, averaged over time interval and
3235 * expressed in radians per second (rad/s).
3236 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
3237 * resolved along body-frame axes, averaged over time interval and
3238 * expressed in radians per second (rad/s).
3239 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
3240 * resolved along body-frame axes, averaged over time interval and
3241 * expressed in radians per second (rad/s).
3242 * @param result instance where new estimated NED frame containing new body position,
3243 * velocity and coordinate transformation matrix will be stored.
3244 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
3245 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
3246 * body-to-NED-frame coordinate transformation matrix are
3247 * invalid.
3248 */
3249 public void navigate(
3250 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
3251 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
3252 final Acceleration fx, final Acceleration fy, final Acceleration fz,
3253 final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result)
3254 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
3255 navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
3256 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
3257 }
3258
3259 /**
3260 * Runs precision local-navigation-frame inertial navigation equations.
3261 * NOTE: only the attitude update and specific force frame transformation
3262 * phases are precise.
3263 *
3264 * @param timeInterval time interval between epochs expressed in seconds (s).
3265 * @param oldPosition previous curvilinear position expressed in terms of latitude,
3266 * longitude and height.
3267 * @param oldC previous body-to-NED coordinate transformation.
3268 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
3269 * along north, east and down axes.
3270 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
3271 * resolved along body-frame axes, averaged over time interval.
3272 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
3273 * resolved along body-frame axes, averaged over time interval.
3274 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
3275 * resolved along body-frame axes, averaged over time interval.
3276 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
3277 * resolved along body-frame axes, averaged over time interval and
3278 * expressed in radians per second (rad/s).
3279 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
3280 * resolved along body-frame axes, averaged over time interval and
3281 * expressed in radians per second (rad/s).
3282 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
3283 * resolved along body-frame axes, averaged over time interval and
3284 * expressed in radians per second (rad/s).
3285 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
3286 * @param result instance where new estimated NED frame containing new body position,
3287 * velocity and coordinate transformation matrix will be stored.
3288 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
3289 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
3290 * body-to-NED-frame coordinate transformation matrix are
3291 * invalid.
3292 */
3293 public void navigate(
3294 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
3295 final NEDVelocity oldVelocity, final Acceleration fx, final Acceleration fy, final Acceleration fz,
3296 final double angularRateX, final double angularRateY, final double angularRateZ,
3297 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
3298 InvalidSourceAndDestinationFrameTypeException {
3299 navigateNED(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
3300 accuracyThreshold, result);
3301 }
3302
3303 /**
3304 * Runs precision local-navigation-frame inertial navigation equations.
3305 * NOTE: only the attitude update and specific force frame transformation
3306 * phases are precise.
3307 *
3308 * @param timeInterval time interval between epochs expressed in seconds (s).
3309 * @param oldPosition previous curvilinear position expressed in terms of latitude,
3310 * longitude and height.
3311 * @param oldC previous body-to-NED coordinate transformation.
3312 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
3313 * along north, east and down axes.
3314 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
3315 * resolved along body-frame axes, averaged over time interval.
3316 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
3317 * resolved along body-frame axes, averaged over time interval.
3318 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
3319 * resolved along body-frame axes, averaged over time interval.
3320 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
3321 * resolved along body-frame axes, averaged over time interval and
3322 * expressed in radians per second (rad/s).
3323 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
3324 * resolved along body-frame axes, averaged over time interval and
3325 * expressed in radians per second (rad/s).
3326 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
3327 * resolved along body-frame axes, averaged over time interval and
3328 * expressed in radians per second (rad/s).
3329 * @param result instance where new estimated NED frame containing new body position,
3330 * velocity and coordinate transformation matrix will be stored.
3331 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
3332 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
3333 * body-to-NED-frame coordinate transformation matrix are
3334 * invalid.
3335 */
3336 public void navigate(
3337 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
3338 final NEDVelocity oldVelocity, final Acceleration fx, final Acceleration fy, final Acceleration fz,
3339 final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result)
3340 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
3341 navigate(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
3342 DEFAULT_ACCURACY_THRESHOLD, result);
3343 }
3344
3345 /**
3346 * Runs precision local-navigation-frame inertial navigation equations.
3347 * NOTE: only the attitude update and specific force frame transformation
3348 * phases are precise.
3349 *
3350 * @param timeInterval time interval between epochs.
3351 * @param oldPosition previous curvilinear position expressed in terms of latitude,
3352 * longitude and height.
3353 * @param oldC previous body-to-NED coordinate transformation.
3354 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
3355 * along north, east and down axes.
3356 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
3357 * resolved along body-frame axes, averaged over time interval.
3358 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
3359 * resolved along body-frame axes, averaged over time interval.
3360 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
3361 * resolved along body-frame axes, averaged over time interval.
3362 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
3363 * resolved along body-frame axes, averaged over time interval and
3364 * expressed in radians per second (rad/s).
3365 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
3366 * resolved along body-frame axes, averaged over time interval and
3367 * expressed in radians per second (rad/s).
3368 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
3369 * resolved along body-frame axes, averaged over time interval and
3370 * expressed in radians per second (rad/s).
3371 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
3372 * @param result instance where new estimated NED frame containing new body position,
3373 * velocity and coordinate transformation matrix will be stored.
3374 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
3375 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
3376 * body-to-NED-frame coordinate transformation matrix are
3377 * invalid.
3378 */
3379 public void navigate(
3380 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
3381 final NEDVelocity oldVelocity, final Acceleration fx, final Acceleration fy, final Acceleration fz,
3382 final double angularRateX, final double angularRateY, final double angularRateZ,
3383 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
3384 InvalidSourceAndDestinationFrameTypeException {
3385 navigateNED(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
3386 accuracyThreshold, result);
3387 }
3388
3389 /**
3390 * Runs precision local-navigation-frame inertial navigation equations.
3391 * NOTE: only the attitude update and specific force frame transformation
3392 * phases are precise.
3393 *
3394 * @param timeInterval time interval between epochs.
3395 * @param oldPosition previous curvilinear position expressed in terms of latitude,
3396 * longitude and height.
3397 * @param oldC previous body-to-NED coordinate transformation.
3398 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
3399 * along north, east and down axes.
3400 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
3401 * resolved along body-frame axes, averaged over time interval.
3402 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
3403 * resolved along body-frame axes, averaged over time interval.
3404 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
3405 * resolved along body-frame axes, averaged over time interval.
3406 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
3407 * resolved along body-frame axes, averaged over time interval and
3408 * expressed in radians per second (rad/s).
3409 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
3410 * resolved along body-frame axes, averaged over time interval and
3411 * expressed in radians per second (rad/s).
3412 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
3413 * resolved along body-frame axes, averaged over time interval and
3414 * expressed in radians per second (rad/s).
3415 * @param result instance where new estimated NED frame containing new body position,
3416 * velocity and coordinate transformation matrix will be stored.
3417 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
3418 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
3419 * body-to-NED-frame coordinate transformation matrix are
3420 * invalid.
3421 */
3422 public void navigate(
3423 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
3424 final NEDVelocity oldVelocity, final Acceleration fx, final Acceleration fy, final Acceleration fz,
3425 final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result)
3426 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
3427 navigateNED(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
3428 DEFAULT_ACCURACY_THRESHOLD, result);
3429 }
3430
3431 /**
3432 * Runs precision local-navigation-frame inertial navigation equations.
3433 * NOTE: only the attitude update and specific force frame transformation
3434 * phases are precise.
3435 *
3436 * @param timeInterval time interval between epochs expressed in seconds (s).
3437 * @param oldLatitude previous latitude expressed in radians (rad).
3438 * @param oldLongitude previous longitude expressed in radians (rad).
3439 * @param oldHeight previous height expressed in meters (m).
3440 * @param oldC previous body-to-NED coordinate transformation.
3441 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
3442 * resolved along NED-frame axes and expressed in meters per second (m/s).
3443 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
3444 * resolved along NED-frame axes and expressed in meters per second (m/s).
3445 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
3446 * resolved along NED-frame axes and expressed in meters per second (m/s).
3447 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
3448 * resolved along body-frame axes, averaged over time interval and
3449 * expressed in meters per squared second (m/s^2).
3450 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
3451 * resolved along body-frame axes, averaged over time interval and
3452 * expressed in meters per squared second (m/s^2).
3453 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
3454 * resolved along body-frame axes, averaged over time interval and
3455 * expressed in meters per squared second (m/s^2).
3456 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
3457 * resolved along body-frame axes, averaged over time interval.
3458 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
3459 * resolved along body-frame axes, averaged over time interval.
3460 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
3461 * resolved along body-frame axes, averaged over time interval.
3462 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
3463 * @param result instance where new estimated NED frame containing new body position,
3464 * velocity and coordinate transformation matrix will be stored.
3465 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
3466 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
3467 * body-to-NED-frame coordinate transformation matrix are
3468 * invalid.
3469 */
3470 public void navigate(
3471 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
3472 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
3473 final double fx, final double fy, final double fz,
3474 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
3475 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
3476 InvalidSourceAndDestinationFrameTypeException {
3477 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
3478 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
3479 }
3480
3481 /**
3482 * Runs precision local-navigation-frame inertial navigation equations.
3483 * NOTE: only the attitude update and specific force frame transformation
3484 * phases are precise.
3485 *
3486 * @param timeInterval time interval between epochs expressed in seconds (s).
3487 * @param oldLatitude previous latitude expressed in radians (rad).
3488 * @param oldLongitude previous longitude expressed in radians (rad).
3489 * @param oldHeight previous height expressed in meters (m).
3490 * @param oldC previous body-to-NED coordinate transformation.
3491 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
3492 * resolved along NED-frame axes and expressed in meters per second (m/s).
3493 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
3494 * resolved along NED-frame axes and expressed in meters per second (m/s).
3495 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
3496 * resolved along NED-frame axes and expressed in meters per second (m/s).
3497 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
3498 * resolved along body-frame axes, averaged over time interval and
3499 * expressed in meters per squared second (m/s^2).
3500 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
3501 * resolved along body-frame axes, averaged over time interval and
3502 * expressed in meters per squared second (m/s^2).
3503 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
3504 * resolved along body-frame axes, averaged over time interval and
3505 * expressed in meters per squared second (m/s^2).
3506 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
3507 * resolved along body-frame axes, averaged over time interval.
3508 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
3509 * resolved along body-frame axes, averaged over time interval.
3510 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
3511 * resolved along body-frame axes, averaged over time interval.
3512 * @param result instance where new estimated NED frame containing new body position,
3513 * velocity and coordinate transformation matrix will be stored.
3514 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
3515 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
3516 * body-to-NED-frame coordinate transformation matrix are
3517 * invalid.
3518 */
3519 public void navigate(
3520 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
3521 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
3522 final double fx, final double fy, final double fz,
3523 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
3524 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
3525 navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
3526 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
3527 }
3528
3529 /**
3530 * Runs precision local-navigation-frame inertial navigation equations.
3531 * NOTE: only the attitude update and specific force frame transformation
3532 * phases are precise.
3533 *
3534 * @param timeInterval time interval between epochs.
3535 * @param oldLatitude previous latitude expressed in radians (rad).
3536 * @param oldLongitude previous longitude expressed in radians (rad).
3537 * @param oldHeight previous height expressed in meters (m).
3538 * @param oldC previous body-to-NED coordinate transformation.
3539 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
3540 * resolved along NED-frame axes and expressed in meters per second (m/s).
3541 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
3542 * resolved along NED-frame axes and expressed in meters per second (m/s).
3543 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
3544 * resolved along NED-frame axes and expressed in meters per second (m/s).
3545 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
3546 * resolved along body-frame axes, averaged over time interval and
3547 * expressed in meters per squared second (m/s^2).
3548 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
3549 * resolved along body-frame axes, averaged over time interval and
3550 * expressed in meters per squared second (m/s^2).
3551 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
3552 * resolved along body-frame axes, averaged over time interval and
3553 * expressed in meters per squared second (m/s^2).
3554 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
3555 * resolved along body-frame axes, averaged over time interval.
3556 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
3557 * resolved along body-frame axes, averaged over time interval.
3558 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
3559 * resolved along body-frame axes, averaged over time interval.
3560 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
3561 * @param result instance where new estimated NED frame containing new body position,
3562 * velocity and coordinate transformation matrix will be stored.
3563 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
3564 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
3565 * body-to-NED-frame coordinate transformation matrix are
3566 * invalid.
3567 */
3568 public void navigate(
3569 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
3570 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
3571 final double fx, final double fy, final double fz,
3572 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
3573 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
3574 InvalidSourceAndDestinationFrameTypeException {
3575 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
3576 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
3577 }
3578
3579 /**
3580 * Runs precision local-navigation-frame inertial navigation equations.
3581 * NOTE: only the attitude update and specific force frame transformation
3582 * phases are precise.
3583 *
3584 * @param timeInterval time interval between epochs.
3585 * @param oldLatitude previous latitude expressed in radians (rad).
3586 * @param oldLongitude previous longitude expressed in radians (rad).
3587 * @param oldHeight previous height expressed in meters (m).
3588 * @param oldC previous body-to-NED coordinate transformation.
3589 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
3590 * resolved along NED-frame axes and expressed in meters per second (m/s).
3591 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
3592 * resolved along NED-frame axes and expressed in meters per second (m/s).
3593 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
3594 * resolved along NED-frame axes and expressed in meters per second (m/s).
3595 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
3596 * resolved along body-frame axes, averaged over time interval and
3597 * expressed in meters per squared second (m/s^2).
3598 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
3599 * resolved along body-frame axes, averaged over time interval and
3600 * expressed in meters per squared second (m/s^2).
3601 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
3602 * resolved along body-frame axes, averaged over time interval and
3603 * expressed in meters per squared second (m/s^2).
3604 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
3605 * resolved along body-frame axes, averaged over time interval.
3606 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
3607 * resolved along body-frame axes, averaged over time interval.
3608 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
3609 * resolved along body-frame axes, averaged over time interval.
3610 * @param result instance where new estimated NED frame containing new body position,
3611 * velocity and coordinate transformation matrix will be stored.
3612 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
3613 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
3614 * body-to-NED-frame coordinate transformation matrix are
3615 * invalid.
3616 */
3617 public void navigate(
3618 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
3619 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
3620 final double fx, final double fy, final double fz,
3621 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
3622 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
3623 navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
3624 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
3625 }
3626
3627 /**
3628 * Runs precision local-navigation-frame inertial navigation equations.
3629 * NOTE: only the attitude update and specific force frame transformation
3630 * phases are precise.
3631 *
3632 * @param timeInterval time interval between epochs expressed in seconds (s).
3633 * @param oldPosition previous curvilinear position expressed in terms of latitude,
3634 * longitude and height.
3635 * @param oldC previous body-to-NED coordinate transformation.
3636 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
3637 * resolved along NED-frame axes and expressed in meters per second (m/s).
3638 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
3639 * resolved along NED-frame axes and expressed in meters per second (m/s).
3640 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
3641 * resolved along NED-frame axes and expressed in meters per second (m/s).
3642 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
3643 * resolved along body-frame axes, averaged over time interval and
3644 * expressed in meters per squared second (m/s^2).
3645 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
3646 * resolved along body-frame axes, averaged over time interval and
3647 * expressed in meters per squared second (m/s^2).
3648 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
3649 * resolved along body-frame axes, averaged over time interval and
3650 * expressed in meters per squared second (m/s^2).
3651 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
3652 * resolved along body-frame axes, averaged over time interval.
3653 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
3654 * resolved along body-frame axes, averaged over time interval.
3655 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
3656 * resolved along body-frame axes, averaged over time interval.
3657 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
3658 * @param result instance where new estimated NED frame containing new body position,
3659 * velocity and coordinate transformation matrix will be stored.
3660 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
3661 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
3662 * body-to-NED-frame coordinate transformation matrix are
3663 * invalid.
3664 */
3665 public void navigate(
3666 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
3667 final double oldVn, final double oldVe, final double oldVd,
3668 final double fx, final double fy, final double fz,
3669 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
3670 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
3671 InvalidSourceAndDestinationFrameTypeException {
3672 navigateNED(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
3673 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
3674 }
3675
3676 /**
3677 * Runs precision local-navigation-frame inertial navigation equations.
3678 * NOTE: only the attitude update and specific force frame transformation
3679 * phases are precise.
3680 *
3681 * @param timeInterval time interval between epochs expressed in seconds (s).
3682 * @param oldPosition previous curvilinear position expressed in terms of latitude,
3683 * longitude and height.
3684 * @param oldC previous body-to-NED coordinate transformation.
3685 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
3686 * resolved along NED-frame axes and expressed in meters per second (m/s).
3687 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
3688 * resolved along NED-frame axes and expressed in meters per second (m/s).
3689 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
3690 * resolved along NED-frame axes and expressed in meters per second (m/s).
3691 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
3692 * resolved along body-frame axes, averaged over time interval and
3693 * expressed in meters per squared second (m/s^2).
3694 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
3695 * resolved along body-frame axes, averaged over time interval and
3696 * expressed in meters per squared second (m/s^2).
3697 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
3698 * resolved along body-frame axes, averaged over time interval and
3699 * expressed in meters per squared second (m/s^2).
3700 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
3701 * resolved along body-frame axes, averaged over time interval.
3702 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
3703 * resolved along body-frame axes, averaged over time interval.
3704 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
3705 * resolved along body-frame axes, averaged over time interval.
3706 * @param result instance where new estimated NED frame containing new body position,
3707 * velocity and coordinate transformation matrix will be stored.
3708 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
3709 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
3710 * body-to-NED-frame coordinate transformation matrix are
3711 * invalid.
3712 */
3713 public void navigate(
3714 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
3715 final double oldVn, final double oldVe, final double oldVd,
3716 final double fx, final double fy, final double fz,
3717 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
3718 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
3719 navigate(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
3720 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
3721 }
3722
3723 /**
3724 * Runs precision local-navigation-frame inertial navigation equations.
3725 * NOTE: only the attitude update and specific force frame transformation
3726 * phases are precise.
3727 *
3728 * @param timeInterval time interval between epochs.
3729 * @param oldPosition previous curvilinear position expressed in terms of latitude,
3730 * longitude and height.
3731 * @param oldC previous body-to-NED coordinate transformation.
3732 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
3733 * resolved along NED-frame axes and expressed in meters per second (m/s).
3734 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
3735 * resolved along NED-frame axes and expressed in meters per second (m/s).
3736 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
3737 * resolved along NED-frame axes and expressed in meters per second (m/s).
3738 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
3739 * resolved along body-frame axes, averaged over time interval and
3740 * expressed in meters per squared second (m/s^2).
3741 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
3742 * resolved along body-frame axes, averaged over time interval and
3743 * expressed in meters per squared second (m/s^2).
3744 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
3745 * resolved along body-frame axes, averaged over time interval and
3746 * expressed in meters per squared second (m/s^2).
3747 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
3748 * resolved along body-frame axes, averaged over time interval.
3749 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
3750 * resolved along body-frame axes, averaged over time interval.
3751 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
3752 * resolved along body-frame axes, averaged over time interval.
3753 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
3754 * @param result instance where new estimated NED frame containing new body position,
3755 * velocity and coordinate transformation matrix will be stored.
3756 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
3757 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
3758 * body-to-NED-frame coordinate transformation matrix are
3759 * invalid.
3760 */
3761 public void navigate(
3762 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
3763 final double oldVn, final double oldVe, final double oldVd,
3764 final double fx, final double fy, final double fz,
3765 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
3766 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
3767 InvalidSourceAndDestinationFrameTypeException {
3768 navigateNED(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
3769 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
3770 }
3771
3772 /**
3773 * Runs precision local-navigation-frame inertial navigation equations.
3774 * NOTE: only the attitude update and specific force frame transformation
3775 * phases are precise.
3776 *
3777 * @param timeInterval time interval between epochs.
3778 * @param oldPosition previous curvilinear position expressed in terms of latitude,
3779 * longitude and height.
3780 * @param oldC previous body-to-NED coordinate transformation.
3781 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
3782 * resolved along NED-frame axes and expressed in meters per second (m/s).
3783 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
3784 * resolved along NED-frame axes and expressed in meters per second (m/s).
3785 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
3786 * resolved along NED-frame axes and expressed in meters per second (m/s).
3787 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
3788 * resolved along body-frame axes, averaged over time interval and
3789 * expressed in meters per squared second (m/s^2).
3790 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
3791 * resolved along body-frame axes, averaged over time interval and
3792 * expressed in meters per squared second (m/s^2).
3793 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
3794 * resolved along body-frame axes, averaged over time interval and
3795 * expressed in meters per squared second (m/s^2).
3796 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
3797 * resolved along body-frame axes, averaged over time interval.
3798 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
3799 * resolved along body-frame axes, averaged over time interval.
3800 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
3801 * resolved along body-frame axes, averaged over time interval.
3802 * @param result instance where new estimated NED frame containing new body position,
3803 * velocity and coordinate transformation matrix will be stored.
3804 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
3805 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
3806 * body-to-NED-frame coordinate transformation matrix are
3807 * invalid.
3808 */
3809 public void navigate(
3810 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
3811 final double oldVn, final double oldVe, final double oldVd,
3812 final double fx, final double fy, final double fz,
3813 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
3814 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
3815 navigate(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
3816 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
3817 }
3818
3819 /**
3820 * Runs precision local-navigation-frame inertial navigation equations.
3821 * NOTE: only the attitude update and specific force frame transformation
3822 * phases are precise.
3823 *
3824 * @param timeInterval time interval between epochs expressed in seconds (s).
3825 * @param oldLatitude previous latitude expressed in radians (rad).
3826 * @param oldLongitude previous longitude expressed in radians (rad).
3827 * @param oldHeight previous height expressed in meters (m).
3828 * @param oldC previous body-to-NED coordinate transformation.
3829 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
3830 * along north, east and down axes.
3831 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
3832 * resolved along body-frame axes, averaged over time interval and
3833 * expressed in meters per squared second (m/s^2).
3834 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
3835 * resolved along body-frame axes, averaged over time interval and
3836 * expressed in meters per squared second (m/s^2).
3837 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
3838 * resolved along body-frame axes, averaged over time interval and
3839 * expressed in meters per squared second (m/s^2).
3840 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
3841 * resolved along body-frame axes, averaged over time interval.
3842 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
3843 * resolved along body-frame axes, averaged over time interval.
3844 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
3845 * resolved along body-frame axes, averaged over time interval.
3846 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
3847 * @param result instance where new estimated NED frame containing new body position,
3848 * velocity and coordinate transformation matrix will be stored.
3849 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
3850 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
3851 * body-to-NED-frame coordinate transformation matrix are
3852 * invalid.
3853 */
3854 public void navigate(
3855 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
3856 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
3857 final double fx, final double fy, final double fz,
3858 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
3859 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
3860 InvalidSourceAndDestinationFrameTypeException {
3861 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
3862 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
3863 }
3864
3865 /**
3866 * Runs precision local-navigation-frame inertial navigation equations.
3867 * NOTE: only the attitude update and specific force frame transformation
3868 * phases are precise.
3869 *
3870 * @param timeInterval time interval between epochs expressed in seconds (s).
3871 * @param oldLatitude previous latitude expressed in radians (rad).
3872 * @param oldLongitude previous longitude expressed in radians (rad).
3873 * @param oldHeight previous height expressed in meters (m).
3874 * @param oldC previous body-to-NED coordinate transformation.
3875 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
3876 * along north, east and down axes.
3877 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
3878 * resolved along body-frame axes, averaged over time interval and
3879 * expressed in meters per squared second (m/s^2).
3880 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
3881 * resolved along body-frame axes, averaged over time interval and
3882 * expressed in meters per squared second (m/s^2).
3883 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
3884 * resolved along body-frame axes, averaged over time interval and
3885 * expressed in meters per squared second (m/s^2).
3886 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
3887 * resolved along body-frame axes, averaged over time interval.
3888 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
3889 * resolved along body-frame axes, averaged over time interval.
3890 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
3891 * resolved along body-frame axes, averaged over time interval.
3892 * @param result instance where new estimated NED frame containing new body position,
3893 * velocity and coordinate transformation matrix will be stored.
3894 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
3895 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
3896 * body-to-NED-frame coordinate transformation matrix are
3897 * invalid.
3898 */
3899 public void navigate(
3900 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
3901 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
3902 final double fx, final double fy, final double fz,
3903 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
3904 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
3905 navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
3906 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
3907 }
3908
3909 /**
3910 * Runs precision local-navigation-frame inertial navigation equations.
3911 * NOTE: only the attitude update and specific force frame transformation
3912 * phases are precise.
3913 *
3914 * @param timeInterval time interval between epochs.
3915 * @param oldLatitude previous latitude expressed in radians (rad).
3916 * @param oldLongitude previous longitude expressed in radians (rad).
3917 * @param oldHeight previous height expressed in meters (m).
3918 * @param oldC previous body-to-NED coordinate transformation.
3919 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
3920 * along north, east and down axes.
3921 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
3922 * resolved along body-frame axes, averaged over time interval and
3923 * expressed in meters per squared second (m/s^2).
3924 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
3925 * resolved along body-frame axes, averaged over time interval and
3926 * expressed in meters per squared second (m/s^2).
3927 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
3928 * resolved along body-frame axes, averaged over time interval and
3929 * expressed in meters per squared second (m/s^2).
3930 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
3931 * resolved along body-frame axes, averaged over time interval.
3932 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
3933 * resolved along body-frame axes, averaged over time interval.
3934 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
3935 * resolved along body-frame axes, averaged over time interval.
3936 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
3937 * @param result instance where new estimated NED frame containing new body position,
3938 * velocity and coordinate transformation matrix will be stored.
3939 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
3940 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
3941 * body-to-NED-frame coordinate transformation matrix are
3942 * invalid.
3943 */
3944 public void navigate(
3945 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
3946 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
3947 final double fx, final double fy, final double fz,
3948 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
3949 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
3950 InvalidSourceAndDestinationFrameTypeException {
3951 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
3952 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
3953 }
3954
3955 /**
3956 * Runs precision local-navigation-frame inertial navigation equations.
3957 * NOTE: only the attitude update and specific force frame transformation
3958 * phases are precise.
3959 *
3960 * @param timeInterval time interval between epochs.
3961 * @param oldLatitude previous latitude expressed in radians (rad).
3962 * @param oldLongitude previous longitude expressed in radians (rad).
3963 * @param oldHeight previous height expressed in meters (m).
3964 * @param oldC previous body-to-NED coordinate transformation.
3965 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
3966 * along north, east and down axes.
3967 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
3968 * resolved along body-frame axes, averaged over time interval and
3969 * expressed in meters per squared second (m/s^2).
3970 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
3971 * resolved along body-frame axes, averaged over time interval and
3972 * expressed in meters per squared second (m/s^2).
3973 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
3974 * resolved along body-frame axes, averaged over time interval and
3975 * expressed in meters per squared second (m/s^2).
3976 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
3977 * resolved along body-frame axes, averaged over time interval.
3978 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
3979 * resolved along body-frame axes, averaged over time interval.
3980 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
3981 * resolved along body-frame axes, averaged over time interval.
3982 * @param result instance where new estimated NED frame containing new body position,
3983 * velocity and coordinate transformation matrix will be stored.
3984 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
3985 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
3986 * body-to-NED-frame coordinate transformation matrix are
3987 * invalid.
3988 */
3989 public void navigate(
3990 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
3991 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
3992 final double fx, final double fy, final double fz,
3993 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
3994 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
3995 navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
3996 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
3997 }
3998
3999 /**
4000 * Runs precision local-navigation-frame inertial navigation equations.
4001 * NOTE: only the attitude update and specific force frame transformation
4002 * phases are precise.
4003 *
4004 * @param timeInterval time interval between epochs expressed in seconds (s).
4005 * @param oldPosition previous curvilinear position expressed in terms of latitude,
4006 * longitude and height.
4007 * @param oldC previous body-to-NED coordinate transformation.
4008 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
4009 * along north, east and down axes.
4010 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
4011 * resolved along body-frame axes, averaged over time interval and
4012 * expressed in meters per squared second (m/s^2).
4013 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
4014 * resolved along body-frame axes, averaged over time interval and
4015 * expressed in meters per squared second (m/s^2).
4016 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
4017 * resolved along body-frame axes, averaged over time interval and
4018 * expressed in meters per squared second (m/s^2).
4019 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
4020 * resolved along body-frame axes, averaged over time interval.
4021 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
4022 * resolved along body-frame axes, averaged over time interval.
4023 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
4024 * resolved along body-frame axes, averaged over time interval.
4025 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
4026 * @param result instance where new estimated NED frame containing new body position,
4027 * velocity and coordinate transformation matrix will be stored.
4028 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
4029 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
4030 * body-to-NED-frame coordinate transformation matrix are
4031 * invalid.
4032 */
4033 public void navigate(
4034 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
4035 final NEDVelocity oldVelocity, final double fx, final double fy, final double fz,
4036 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
4037 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
4038 InvalidSourceAndDestinationFrameTypeException {
4039 navigateNED(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
4040 accuracyThreshold, result);
4041 }
4042
4043 /**
4044 * Runs precision local-navigation-frame inertial navigation equations.
4045 * NOTE: only the attitude update and specific force frame transformation
4046 * phases are precise.
4047 *
4048 * @param timeInterval time interval between epochs expressed in seconds (s).
4049 * @param oldPosition previous curvilinear position expressed in terms of latitude,
4050 * longitude and height.
4051 * @param oldC previous body-to-NED coordinate transformation.
4052 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
4053 * along north, east and down axes.
4054 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
4055 * resolved along body-frame axes, averaged over time interval and
4056 * expressed in meters per squared second (m/s^2).
4057 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
4058 * resolved along body-frame axes, averaged over time interval and
4059 * expressed in meters per squared second (m/s^2).
4060 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
4061 * resolved along body-frame axes, averaged over time interval and
4062 * expressed in meters per squared second (m/s^2).
4063 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
4064 * resolved along body-frame axes, averaged over time interval.
4065 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
4066 * resolved along body-frame axes, averaged over time interval.
4067 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
4068 * resolved along body-frame axes, averaged over time interval.
4069 * @param result instance where new estimated NED frame containing new body position,
4070 * velocity and coordinate transformation matrix will be stored.
4071 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
4072 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
4073 * body-to-NED-frame coordinate transformation matrix are
4074 * invalid.
4075 */
4076 public void navigate(
4077 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
4078 final NEDVelocity oldVelocity, final double fx, final double fy, final double fz,
4079 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
4080 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
4081 navigate(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
4082 DEFAULT_ACCURACY_THRESHOLD, result);
4083 }
4084
4085 /**
4086 * Runs precision local-navigation-frame inertial navigation equations.
4087 * NOTE: only the attitude update and specific force frame transformation
4088 * phases are precise.
4089 *
4090 * @param timeInterval time interval between epochs.
4091 * @param oldPosition previous curvilinear position expressed in terms of latitude,
4092 * longitude and height.
4093 * @param oldC previous body-to-NED coordinate transformation.
4094 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
4095 * along north, east and down axes.
4096 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
4097 * resolved along body-frame axes, averaged over time interval and
4098 * expressed in meters per squared second (m/s^2).
4099 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
4100 * resolved along body-frame axes, averaged over time interval and
4101 * expressed in meters per squared second (m/s^2).
4102 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
4103 * resolved along body-frame axes, averaged over time interval and
4104 * expressed in meters per squared second (m/s^2).
4105 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
4106 * resolved along body-frame axes, averaged over time interval.
4107 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
4108 * resolved along body-frame axes, averaged over time interval.
4109 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
4110 * resolved along body-frame axes, averaged over time interval.
4111 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
4112 * @param result instance where new estimated NED frame containing new body position,
4113 * velocity and coordinate transformation matrix will be stored.
4114 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
4115 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
4116 * body-to-NED-frame coordinate transformation matrix are
4117 * invalid.
4118 */
4119 public void navigate(
4120 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
4121 final NEDVelocity oldVelocity, final double fx, final double fy, final double fz,
4122 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
4123 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
4124 InvalidSourceAndDestinationFrameTypeException {
4125 navigateNED(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
4126 accuracyThreshold, result);
4127 }
4128
4129 /**
4130 * Runs precision local-navigation-frame inertial navigation equations.
4131 * NOTE: only the attitude update and specific force frame transformation
4132 * phases are precise.
4133 *
4134 * @param timeInterval time interval between epochs.
4135 * @param oldPosition previous curvilinear position expressed in terms of latitude,
4136 * longitude and height.
4137 * @param oldC previous body-to-NED coordinate transformation.
4138 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
4139 * along north, east and down axes.
4140 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
4141 * resolved along body-frame axes, averaged over time interval and
4142 * expressed in meters per squared second (m/s^2).
4143 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
4144 * resolved along body-frame axes, averaged over time interval and
4145 * expressed in meters per squared second (m/s^2).
4146 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
4147 * resolved along body-frame axes, averaged over time interval and
4148 * expressed in meters per squared second (m/s^2).
4149 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
4150 * resolved along body-frame axes, averaged over time interval.
4151 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
4152 * resolved along body-frame axes, averaged over time interval.
4153 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
4154 * resolved along body-frame axes, averaged over time interval.
4155 * @param result instance where new estimated NED frame containing new body position,
4156 * velocity and coordinate transformation matrix will be stored.
4157 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
4158 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
4159 * body-to-NED-frame coordinate transformation matrix are
4160 * invalid.
4161 */
4162 public void navigate(
4163 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
4164 final NEDVelocity oldVelocity, final double fx, final double fy, final double fz,
4165 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
4166 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
4167 navigate(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
4168 DEFAULT_ACCURACY_THRESHOLD, result);
4169 }
4170
4171 /**
4172 * Runs precision local-navigation-frame inertial navigation equations.
4173 * NOTE: only the attitude update and specific force frame transformation
4174 * phases are precise.
4175 *
4176 * @param timeInterval time interval between epochs expressed in seconds (s).
4177 * @param oldLatitude previous latitude angle.
4178 * @param oldLongitude previous longitude angle.
4179 * @param oldHeight previous height.
4180 * @param oldC previous body-to-NED coordinate transformation.
4181 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
4182 * resolved along NED-frame axes.
4183 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
4184 * resolved along NED-frame axes.
4185 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
4186 * resolved along NED-frame axes.
4187 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
4188 * resolved along body-frame axes, averaged over time interval and
4189 * expressed in meters per squared second (m/s^2).
4190 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
4191 * resolved along body-frame axes, averaged over time interval and
4192 * expressed in meters per squared second (m/s^2).
4193 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
4194 * resolved along body-frame axes, averaged over time interval and
4195 * expressed in meters per squared second (m/s^2).
4196 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
4197 * resolved along body-frame axes, averaged over time interval and
4198 * expressed in radians per second (rad/s).
4199 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
4200 * resolved along body-frame axes, averaged over time interval and
4201 * expressed in radians per second (rad/s).
4202 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
4203 * resolved along body-frame axes, averaged over time interval and
4204 * expressed in radians per second (rad/s).
4205 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
4206 * @param result instance where new estimated NED frame containing new body position,
4207 * velocity and coordinate transformation matrix will be stored.
4208 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
4209 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
4210 * body-to-NED-frame coordinate transformation matrix are
4211 * invalid.
4212 */
4213 public void navigate(
4214 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
4215 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
4216 final double fx, final double fy, final double fz,
4217 final double angularRateX, final double angularRateY, final double angularRateZ,
4218 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
4219 InvalidSourceAndDestinationFrameTypeException {
4220 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
4221 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
4222 }
4223
4224 /**
4225 * Runs precision local-navigation-frame inertial navigation equations.
4226 * NOTE: only the attitude update and specific force frame transformation
4227 * phases are precise.
4228 *
4229 * @param timeInterval time interval between epochs expressed in seconds (s).
4230 * @param oldLatitude previous latitude angle.
4231 * @param oldLongitude previous longitude angle.
4232 * @param oldHeight previous height.
4233 * @param oldC previous body-to-NED coordinate transformation.
4234 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
4235 * resolved along NED-frame axes.
4236 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
4237 * resolved along NED-frame axes.
4238 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
4239 * resolved along NED-frame axes.
4240 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
4241 * resolved along body-frame axes, averaged over time interval and
4242 * expressed in meters per squared second (m/s^2).
4243 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
4244 * resolved along body-frame axes, averaged over time interval and
4245 * expressed in meters per squared second (m/s^2).
4246 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
4247 * resolved along body-frame axes, averaged over time interval and
4248 * expressed in meters per squared second (m/s^2).
4249 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
4250 * resolved along body-frame axes, averaged over time interval and
4251 * expressed in radians per second (rad/s).
4252 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
4253 * resolved along body-frame axes, averaged over time interval and
4254 * expressed in radians per second (rad/s).
4255 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
4256 * resolved along body-frame axes, averaged over time interval and
4257 * expressed in radians per second (rad/s).
4258 * @param result instance where new estimated NED frame containing new body position,
4259 * velocity and coordinate transformation matrix will be stored.
4260 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
4261 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
4262 * body-to-NED-frame coordinate transformation matrix are
4263 * invalid.
4264 */
4265 public void navigate(
4266 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
4267 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
4268 final double fx, final double fy, final double fz,
4269 final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result)
4270 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
4271 navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
4272 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
4273 }
4274
4275 /**
4276 * Runs precision local-navigation-frame inertial navigation equations.
4277 * NOTE: only the attitude update and specific force frame transformation
4278 * phases are precise.
4279 *
4280 * @param timeInterval time interval between epochs.
4281 * @param oldLatitude previous latitude angle.
4282 * @param oldLongitude previous longitude angle.
4283 * @param oldHeight previous height.
4284 * @param oldC previous body-to-NED coordinate transformation.
4285 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
4286 * resolved along NED-frame axes.
4287 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
4288 * resolved along NED-frame axes.
4289 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
4290 * resolved along NED-frame axes.
4291 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
4292 * resolved along body-frame axes, averaged over time interval and
4293 * expressed in meters per squared second (m/s^2).
4294 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
4295 * resolved along body-frame axes, averaged over time interval and
4296 * expressed in meters per squared second (m/s^2).
4297 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
4298 * resolved along body-frame axes, averaged over time interval and
4299 * expressed in meters per squared second (m/s^2).
4300 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
4301 * resolved along body-frame axes, averaged over time interval and
4302 * expressed in radians per second (rad/s).
4303 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
4304 * resolved along body-frame axes, averaged over time interval and
4305 * expressed in radians per second (rad/s).
4306 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
4307 * resolved along body-frame axes, averaged over time interval and
4308 * expressed in radians per second (rad/s).
4309 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
4310 * @param result instance where new estimated NED frame containing new body position,
4311 * velocity and coordinate transformation matrix will be stored.
4312 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
4313 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
4314 * body-to-NED-frame coordinate transformation matrix are
4315 * invalid.
4316 */
4317 public void navigate(
4318 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
4319 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
4320 final double fx, final double fy, final double fz,
4321 final double angularRateX, final double angularRateY, final double angularRateZ,
4322 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
4323 InvalidSourceAndDestinationFrameTypeException {
4324 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
4325 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
4326 }
4327
4328 /**
4329 * Runs precision local-navigation-frame inertial navigation equations.
4330 * NOTE: only the attitude update and specific force frame transformation
4331 * phases are precise.
4332 *
4333 * @param timeInterval time interval between epochs.
4334 * @param oldLatitude previous latitude angle.
4335 * @param oldLongitude previous longitude angle.
4336 * @param oldHeight previous height.
4337 * @param oldC previous body-to-NED coordinate transformation.
4338 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
4339 * resolved along NED-frame axes.
4340 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
4341 * resolved along NED-frame axes.
4342 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
4343 * resolved along NED-frame axes.
4344 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
4345 * resolved along body-frame axes, averaged over time interval and
4346 * expressed in meters per squared second (m/s^2).
4347 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
4348 * resolved along body-frame axes, averaged over time interval and
4349 * expressed in meters per squared second (m/s^2).
4350 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
4351 * resolved along body-frame axes, averaged over time interval and
4352 * expressed in meters per squared second (m/s^2).
4353 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
4354 * resolved along body-frame axes, averaged over time interval and
4355 * expressed in radians per second (rad/s).
4356 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
4357 * resolved along body-frame axes, averaged over time interval and
4358 * expressed in radians per second (rad/s).
4359 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
4360 * resolved along body-frame axes, averaged over time interval and
4361 * expressed in radians per second (rad/s).
4362 * @param result instance where new estimated NED frame containing new body position,
4363 * velocity and coordinate transformation matrix will be stored.
4364 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
4365 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
4366 * body-to-NED-frame coordinate transformation matrix are
4367 * invalid.
4368 */
4369 public void navigate(
4370 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
4371 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
4372 final double fx, final double fy, final double fz,
4373 final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result)
4374 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
4375 navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
4376 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
4377 }
4378
4379 /**
4380 * Runs precision local-navigation-frame inertial navigation equations.
4381 * NOTE: only the attitude update and specific force frame transformation
4382 * phases are precise.
4383 *
4384 * @param timeInterval time interval between epochs expressed in seconds (s).
4385 * @param oldLatitude previous latitude angle.
4386 * @param oldLongitude previous longitude angle.
4387 * @param oldHeight previous height.
4388 * @param oldC previous body-to-NED coordinate transformation.
4389 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
4390 * resolved along NED-frame axes.
4391 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
4392 * resolved along NED-frame axes.
4393 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
4394 * resolved along NED-frame axes.
4395 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
4396 * resolved along body-frame axes, averaged over time interval.
4397 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
4398 * resolved along body-frame axes, averaged over time interval.
4399 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
4400 * resolved along body-frame axes, averaged over time interval.
4401 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
4402 * resolved along body-frame axes, averaged over time interval.
4403 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
4404 * resolved along body-frame axes, averaged over time interval.
4405 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
4406 * resolved along body-frame axes, averaged over time interval.
4407 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
4408 * @param result instance where new estimated NED frame containing new body position,
4409 * velocity and coordinate transformation matrix will be stored.
4410 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
4411 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
4412 * body-to-NED-frame coordinate transformation matrix are
4413 * invalid.
4414 */
4415 public void navigate(
4416 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
4417 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
4418 final Acceleration fx, final Acceleration fy, final Acceleration fz,
4419 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
4420 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
4421 InvalidSourceAndDestinationFrameTypeException {
4422 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
4423 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
4424 }
4425
4426 /**
4427 * Runs precision local-navigation-frame inertial navigation equations.
4428 * NOTE: only the attitude update and specific force frame transformation
4429 * phases are precise.
4430 *
4431 * @param timeInterval time interval between epochs expressed in seconds (s).
4432 * @param oldLatitude previous latitude angle.
4433 * @param oldLongitude previous longitude angle.
4434 * @param oldHeight previous height.
4435 * @param oldC previous body-to-NED coordinate transformation.
4436 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
4437 * resolved along NED-frame axes.
4438 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
4439 * resolved along NED-frame axes.
4440 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
4441 * resolved along NED-frame axes.
4442 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
4443 * resolved along body-frame axes, averaged over time interval.
4444 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
4445 * resolved along body-frame axes, averaged over time interval.
4446 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
4447 * resolved along body-frame axes, averaged over time interval.
4448 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
4449 * resolved along body-frame axes, averaged over time interval.
4450 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
4451 * resolved along body-frame axes, averaged over time interval.
4452 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
4453 * resolved along body-frame axes, averaged over time interval.
4454 * @param result instance where new estimated NED frame containing new body position,
4455 * velocity and coordinate transformation matrix will be stored.
4456 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
4457 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
4458 * body-to-NED-frame coordinate transformation matrix are
4459 * invalid.
4460 */
4461 public void navigate(
4462 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
4463 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
4464 final Acceleration fx, final Acceleration fy, final Acceleration fz,
4465 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
4466 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
4467 navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
4468 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
4469 }
4470
4471 /**
4472 * Runs precision local-navigation-frame inertial navigation equations.
4473 * NOTE: only the attitude update and specific force frame transformation
4474 * phases are precise.
4475 *
4476 * @param timeInterval time interval between epochs.
4477 * @param oldLatitude previous latitude angle.
4478 * @param oldLongitude previous longitude angle.
4479 * @param oldHeight previous height.
4480 * @param oldC previous body-to-NED coordinate transformation.
4481 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
4482 * resolved along NED-frame axes.
4483 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
4484 * resolved along NED-frame axes.
4485 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
4486 * resolved along NED-frame axes.
4487 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
4488 * resolved along body-frame axes, averaged over time interval.
4489 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
4490 * resolved along body-frame axes, averaged over time interval.
4491 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
4492 * resolved along body-frame axes, averaged over time interval.
4493 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
4494 * resolved along body-frame axes, averaged over time interval.
4495 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
4496 * resolved along body-frame axes, averaged over time interval.
4497 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
4498 * resolved along body-frame axes, averaged over time interval.
4499 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
4500 * @param result instance where new estimated NED frame containing new body position,
4501 * velocity and coordinate transformation matrix will be stored.
4502 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
4503 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
4504 * body-to-NED-frame coordinate transformation matrix are
4505 * invalid.
4506 */
4507 public void navigate(
4508 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
4509 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
4510 final Acceleration fx, final Acceleration fy, final Acceleration fz,
4511 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
4512 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
4513 InvalidSourceAndDestinationFrameTypeException {
4514 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
4515 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
4516 }
4517
4518 /**
4519 * Runs precision local-navigation-frame inertial navigation equations.
4520 * NOTE: only the attitude update and specific force frame transformation
4521 * phases are precise.
4522 *
4523 * @param timeInterval time interval between epochs.
4524 * @param oldLatitude previous latitude angle.
4525 * @param oldLongitude previous longitude angle.
4526 * @param oldHeight previous height.
4527 * @param oldC previous body-to-NED coordinate transformation.
4528 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
4529 * resolved along NED-frame axes.
4530 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
4531 * resolved along NED-frame axes.
4532 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
4533 * resolved along NED-frame axes.
4534 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
4535 * resolved along body-frame axes, averaged over time interval.
4536 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
4537 * resolved along body-frame axes, averaged over time interval.
4538 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
4539 * resolved along body-frame axes, averaged over time interval.
4540 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
4541 * resolved along body-frame axes, averaged over time interval.
4542 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
4543 * resolved along body-frame axes, averaged over time interval.
4544 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
4545 * resolved along body-frame axes, averaged over time interval.
4546 * @param result instance where new estimated NED frame containing new body position,
4547 * velocity and coordinate transformation matrix will be stored.
4548 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
4549 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
4550 * body-to-NED-frame coordinate transformation matrix are
4551 * invalid.
4552 */
4553 public void navigate(
4554 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
4555 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
4556 final Acceleration fx, final Acceleration fy, final Acceleration fz,
4557 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
4558 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
4559 navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
4560 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
4561 }
4562
4563 /**
4564 * Runs precision local-navigation-frame inertial navigation equations.
4565 * NOTE: only the attitude update and specific force frame transformation
4566 * phases are precise.
4567 *
4568 * @param timeInterval time interval between epochs expressed in seconds (s).
4569 * @param oldLatitude previous latitude expressed in radians (rad).
4570 * @param oldLongitude previous longitude expressed in radians (rad).
4571 * @param oldHeight previous height expressed in meters (m).
4572 * @param oldC previous body-to-NED coordinate transformation.
4573 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
4574 * resolved along NED-frame axes.
4575 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
4576 * resolved along NED-frame axes.
4577 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
4578 * resolved along NED-frame axes.
4579 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
4580 * resolved along body-frame axes, averaged over time interval.
4581 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
4582 * resolved along body-frame axes, averaged over time interval.
4583 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
4584 * resolved along body-frame axes, averaged over time interval.
4585 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
4586 * resolved along body-frame axes, averaged over time interval.
4587 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
4588 * resolved along body-frame axes, averaged over time interval.
4589 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
4590 * resolved along body-frame axes, averaged over time interval.
4591 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
4592 * @param result instance where new estimated NED frame containing new body position,
4593 * velocity and coordinate transformation matrix will be stored.
4594 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
4595 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
4596 * body-to-NED-frame coordinate transformation matrix are
4597 * invalid.
4598 */
4599 public void navigate(
4600 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
4601 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
4602 final Acceleration fx, final Acceleration fy, final Acceleration fz,
4603 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
4604 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
4605 InvalidSourceAndDestinationFrameTypeException {
4606 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
4607 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
4608 }
4609
4610 /**
4611 * Runs precision local-navigation-frame inertial navigation equations.
4612 * NOTE: only the attitude update and specific force frame transformation
4613 * phases are precise.
4614 *
4615 * @param timeInterval time interval between epochs expressed in seconds (s).
4616 * @param oldLatitude previous latitude expressed in radians (rad).
4617 * @param oldLongitude previous longitude expressed in radians (rad).
4618 * @param oldHeight previous height expressed in meters (m).
4619 * @param oldC previous body-to-NED coordinate transformation.
4620 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
4621 * resolved along NED-frame axes.
4622 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
4623 * resolved along NED-frame axes.
4624 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
4625 * resolved along NED-frame axes.
4626 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
4627 * resolved along body-frame axes, averaged over time interval.
4628 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
4629 * resolved along body-frame axes, averaged over time interval.
4630 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
4631 * resolved along body-frame axes, averaged over time interval.
4632 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
4633 * resolved along body-frame axes, averaged over time interval.
4634 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
4635 * resolved along body-frame axes, averaged over time interval.
4636 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
4637 * resolved along body-frame axes, averaged over time interval.
4638 * @param result instance where new estimated NED frame containing new body position,
4639 * velocity and coordinate transformation matrix will be stored.
4640 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
4641 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
4642 * body-to-NED-frame coordinate transformation matrix are
4643 * invalid.
4644 */
4645 public void navigate(
4646 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
4647 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
4648 final Acceleration fx, final Acceleration fy, final Acceleration fz,
4649 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
4650 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
4651 navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
4652 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
4653 }
4654
4655 /**
4656 * Runs precision local-navigation-frame inertial navigation equations.
4657 * NOTE: only the attitude update and specific force frame transformation
4658 * phases are precise.
4659 *
4660 * @param timeInterval time interval between epochs.
4661 * @param oldLatitude previous latitude expressed in radians (rad).
4662 * @param oldLongitude previous longitude expressed in radians (rad).
4663 * @param oldHeight previous height expressed in meters (m).
4664 * @param oldC previous body-to-NED coordinate transformation.
4665 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
4666 * resolved along NED-frame axes.
4667 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
4668 * resolved along NED-frame axes.
4669 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
4670 * resolved along NED-frame axes.
4671 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
4672 * resolved along body-frame axes, averaged over time interval.
4673 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
4674 * resolved along body-frame axes, averaged over time interval.
4675 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
4676 * resolved along body-frame axes, averaged over time interval.
4677 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
4678 * resolved along body-frame axes, averaged over time interval.
4679 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
4680 * resolved along body-frame axes, averaged over time interval.
4681 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
4682 * resolved along body-frame axes, averaged over time interval.
4683 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
4684 * @param result instance where new estimated NED frame containing new body position,
4685 * velocity and coordinate transformation matrix will be stored.
4686 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
4687 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
4688 * body-to-NED-frame coordinate transformation matrix are
4689 * invalid.
4690 */
4691 public void navigate(
4692 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
4693 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
4694 final Acceleration fx, final Acceleration fy, final Acceleration fz,
4695 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
4696 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
4697 InvalidSourceAndDestinationFrameTypeException {
4698 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
4699 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
4700 }
4701
4702 /**
4703 * Runs precision local-navigation-frame inertial navigation equations.
4704 * NOTE: only the attitude update and specific force frame transformation
4705 * phases are precise.
4706 *
4707 * @param timeInterval time interval between epochs.
4708 * @param oldLatitude previous latitude expressed in radians (rad).
4709 * @param oldLongitude previous longitude expressed in radians (rad).
4710 * @param oldHeight previous height expressed in meters (m).
4711 * @param oldC previous body-to-NED coordinate transformation.
4712 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
4713 * resolved along NED-frame axes.
4714 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
4715 * resolved along NED-frame axes.
4716 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
4717 * resolved along NED-frame axes.
4718 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
4719 * resolved along body-frame axes, averaged over time interval.
4720 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
4721 * resolved along body-frame axes, averaged over time interval.
4722 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
4723 * resolved along body-frame axes, averaged over time interval.
4724 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
4725 * resolved along body-frame axes, averaged over time interval.
4726 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
4727 * resolved along body-frame axes, averaged over time interval.
4728 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
4729 * resolved along body-frame axes, averaged over time interval.
4730 * @param result instance where new estimated NED frame containing new body position,
4731 * velocity and coordinate transformation matrix will be stored.
4732 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
4733 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
4734 * body-to-NED-frame coordinate transformation matrix are
4735 * invalid.
4736 */
4737 public void navigate(
4738 final Time timeInterval, final double oldLatitude, final double oldLongitude,
4739 final double oldHeight, final CoordinateTransformation oldC,
4740 final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
4741 final Acceleration fx, final Acceleration fy, final Acceleration fz,
4742 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
4743 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
4744 navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
4745 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
4746 }
4747
4748 /**
4749 * Runs precision local-navigation-frame inertial navigation equations.
4750 * NOTE: only the attitude update and specific force frame transformation
4751 * phases are precise.
4752 *
4753 * @param timeInterval time interval between epochs expressed in seconds (s).
4754 * @param oldPosition previous curvilinear position expressed in terms of latitude,
4755 * longitude and height.
4756 * @param oldC previous body-to-NED coordinate transformation.
4757 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
4758 * resolved along NED-frame axes.
4759 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
4760 * resolved along NED-frame axes.
4761 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
4762 * resolved along NED-frame axes.
4763 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
4764 * resolved along body-frame axes, averaged over time interval.
4765 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
4766 * resolved along body-frame axes, averaged over time interval.
4767 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
4768 * resolved along body-frame axes, averaged over time interval.
4769 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
4770 * resolved along body-frame axes, averaged over time interval.
4771 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
4772 * resolved along body-frame axes, averaged over time interval.
4773 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
4774 * resolved along body-frame axes, averaged over time interval.
4775 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
4776 * @param result instance where new estimated NED frame containing new body position,
4777 * velocity and coordinate transformation matrix will be stored.
4778 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
4779 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
4780 * body-to-NED-frame coordinate transformation matrix are
4781 * invalid.
4782 */
4783 public void navigate(
4784 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
4785 final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
4786 final Acceleration fx, final Acceleration fy, final Acceleration fz,
4787 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
4788 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
4789 InvalidSourceAndDestinationFrameTypeException {
4790 navigateNED(timeInterval, oldPosition, oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
4791 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
4792 }
4793
4794 /**
4795 * Runs precision local-navigation-frame inertial navigation equations.
4796 * NOTE: only the attitude update and specific force frame transformation
4797 * phases are precise.
4798 *
4799 * @param timeInterval time interval between epochs expressed in seconds (s).
4800 * @param oldPosition previous curvilinear position expressed in terms of latitude,
4801 * longitude and height.
4802 * @param oldC previous body-to-NED coordinate transformation.
4803 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
4804 * resolved along NED-frame axes.
4805 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
4806 * resolved along NED-frame axes.
4807 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
4808 * resolved along NED-frame axes.
4809 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
4810 * resolved along body-frame axes, averaged over time interval.
4811 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
4812 * resolved along body-frame axes, averaged over time interval.
4813 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
4814 * resolved along body-frame axes, averaged over time interval.
4815 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
4816 * resolved along body-frame axes, averaged over time interval.
4817 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
4818 * resolved along body-frame axes, averaged over time interval.
4819 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
4820 * resolved along body-frame axes, averaged over time interval.
4821 * @param result instance where new estimated NED frame containing new body position,
4822 * velocity and coordinate transformation matrix will be stored.
4823 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
4824 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
4825 * body-to-NED-frame coordinate transformation matrix are
4826 * invalid.
4827 */
4828 public void navigate(
4829 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
4830 final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
4831 final Acceleration fx, final Acceleration fy, final Acceleration fz,
4832 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
4833 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
4834 navigate(timeInterval, oldPosition, oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
4835 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
4836 }
4837
4838 /**
4839 * Runs precision local-navigation-frame inertial navigation equations.
4840 * NOTE: only the attitude update and specific force frame transformation
4841 * phases are precise.
4842 *
4843 * @param timeInterval time interval between epochs.
4844 * @param oldPosition previous curvilinear position expressed in terms of latitude,
4845 * longitude and height.
4846 * @param oldC previous body-to-NED coordinate transformation.
4847 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
4848 * resolved along NED-frame axes.
4849 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
4850 * resolved along NED-frame axes.
4851 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
4852 * resolved along NED-frame axes.
4853 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
4854 * resolved along body-frame axes, averaged over time interval.
4855 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
4856 * resolved along body-frame axes, averaged over time interval.
4857 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
4858 * resolved along body-frame axes, averaged over time interval.
4859 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
4860 * resolved along body-frame axes, averaged over time interval.
4861 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
4862 * resolved along body-frame axes, averaged over time interval.
4863 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
4864 * resolved along body-frame axes, averaged over time interval.
4865 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
4866 * @param result instance where new estimated NED frame containing new body position,
4867 * velocity and coordinate transformation matrix will be stored.
4868 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
4869 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
4870 * body-to-NED-frame coordinate transformation matrix are
4871 * invalid.
4872 */
4873 public void navigate(
4874 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
4875 final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
4876 final Acceleration fx, final Acceleration fy, final Acceleration fz,
4877 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
4878 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
4879 InvalidSourceAndDestinationFrameTypeException {
4880 navigateNED(timeInterval, oldPosition, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
4881 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
4882 }
4883
4884 /**
4885 * Runs precision local-navigation-frame inertial navigation equations.
4886 * NOTE: only the attitude update and specific force frame transformation
4887 * phases are precise.
4888 *
4889 * @param timeInterval time interval between epochs.
4890 * @param oldPosition previous curvilinear position expressed in terms of latitude,
4891 * longitude and height.
4892 * @param oldC previous body-to-NED coordinate transformation.
4893 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
4894 * resolved along NED-frame axes.
4895 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
4896 * resolved along NED-frame axes.
4897 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
4898 * resolved along NED-frame axes.
4899 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
4900 * resolved along body-frame axes, averaged over time interval.
4901 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
4902 * resolved along body-frame axes, averaged over time interval.
4903 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
4904 * resolved along body-frame axes, averaged over time interval.
4905 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
4906 * resolved along body-frame axes, averaged over time interval.
4907 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
4908 * resolved along body-frame axes, averaged over time interval.
4909 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
4910 * resolved along body-frame axes, averaged over time interval.
4911 * @param result instance where new estimated NED frame containing new body position,
4912 * velocity and coordinate transformation matrix will be stored.
4913 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
4914 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
4915 * body-to-NED-frame coordinate transformation matrix are
4916 * invalid.
4917 */
4918 public void navigate(
4919 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
4920 final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
4921 final Acceleration fx, final Acceleration fy, final Acceleration fz,
4922 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
4923 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
4924 navigate(timeInterval, oldPosition, oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
4925 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
4926 }
4927
4928 /**
4929 * Runs precision local-navigation-frame inertial navigation equations.
4930 * NOTE: only the attitude update and specific force frame transformation
4931 * phases are precise.
4932 *
4933 * @param timeInterval time interval between epochs expressed in seconds (s).
4934 * @param oldLatitude previous latitude angle.
4935 * @param oldLongitude previous longitude angle.
4936 * @param oldHeight previous height.
4937 * @param oldC previous body-to-NED coordinate transformation.
4938 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
4939 * along north, east and down axes.
4940 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
4941 * resolved along body-frame axes, averaged over time interval.
4942 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
4943 * resolved along body-frame axes, averaged over time interval.
4944 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
4945 * resolved along body-frame axes, averaged over time interval.
4946 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
4947 * resolved along body-frame axes, averaged over time interval.
4948 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
4949 * resolved along body-frame axes, averaged over time interval.
4950 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
4951 * resolved along body-frame axes, averaged over time interval.
4952 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
4953 * @param result instance where new estimated NED frame containing new body position,
4954 * velocity and coordinate transformation matrix will be stored.
4955 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
4956 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
4957 * body-to-NED-frame coordinate transformation matrix are
4958 * invalid.
4959 */
4960 public void navigate(
4961 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
4962 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
4963 final Acceleration fx, final Acceleration fy, final Acceleration fz,
4964 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
4965 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
4966 InvalidSourceAndDestinationFrameTypeException {
4967 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
4968 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
4969 }
4970
4971 /**
4972 * Runs precision local-navigation-frame inertial navigation equations.
4973 * NOTE: only the attitude update and specific force frame transformation
4974 * phases are precise.
4975 *
4976 * @param timeInterval time interval between epochs expressed in seconds (s).
4977 * @param oldLatitude previous latitude angle.
4978 * @param oldLongitude previous longitude angle.
4979 * @param oldHeight previous height.
4980 * @param oldC previous body-to-NED coordinate transformation.
4981 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
4982 * along north, east and down axes.
4983 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
4984 * resolved along body-frame axes, averaged over time interval.
4985 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
4986 * resolved along body-frame axes, averaged over time interval.
4987 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
4988 * resolved along body-frame axes, averaged over time interval.
4989 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
4990 * resolved along body-frame axes, averaged over time interval.
4991 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
4992 * resolved along body-frame axes, averaged over time interval.
4993 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
4994 * resolved along body-frame axes, averaged over time interval.
4995 * @param result instance where new estimated NED frame containing new body position,
4996 * velocity and coordinate transformation matrix will be stored.
4997 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
4998 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
4999 * body-to-NED-frame coordinate transformation matrix are
5000 * invalid.
5001 */
5002 public void navigate(
5003 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
5004 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
5005 final Acceleration fx, final Acceleration fy, final Acceleration fz,
5006 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
5007 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
5008 navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
5009 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
5010 }
5011
5012 /**
5013 * Runs precision local-navigation-frame inertial navigation equations.
5014 * NOTE: only the attitude update and specific force frame transformation
5015 * phases are precise.
5016 *
5017 * @param timeInterval time interval between epochs.
5018 * @param oldLatitude previous latitude angle.
5019 * @param oldLongitude previous longitude angle.
5020 * @param oldHeight previous height.
5021 * @param oldC previous body-to-NED coordinate transformation.
5022 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
5023 * along north, east and down axes.
5024 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
5025 * resolved along body-frame axes, averaged over time interval.
5026 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
5027 * resolved along body-frame axes, averaged over time interval.
5028 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
5029 * resolved along body-frame axes, averaged over time interval.
5030 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
5031 * resolved along body-frame axes, averaged over time interval.
5032 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
5033 * resolved along body-frame axes, averaged over time interval.
5034 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
5035 * resolved along body-frame axes, averaged over time interval.
5036 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
5037 * @param result instance where new estimated NED frame containing new body position,
5038 * velocity and coordinate transformation matrix will be stored.
5039 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
5040 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
5041 * body-to-NED-frame coordinate transformation matrix are
5042 * invalid.
5043 */
5044 public void navigate(
5045 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
5046 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
5047 final Acceleration fx, final Acceleration fy, final Acceleration fz,
5048 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
5049 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
5050 InvalidSourceAndDestinationFrameTypeException {
5051 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
5052 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
5053 }
5054
5055 /**
5056 * Runs precision local-navigation-frame inertial navigation equations.
5057 * NOTE: only the attitude update and specific force frame transformation
5058 * phases are precise.
5059 *
5060 * @param timeInterval time interval between epochs.
5061 * @param oldLatitude previous latitude angle.
5062 * @param oldLongitude previous longitude angle.
5063 * @param oldHeight previous height.
5064 * @param oldC previous body-to-NED coordinate transformation.
5065 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
5066 * along north, east and down axes.
5067 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
5068 * resolved along body-frame axes, averaged over time interval.
5069 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
5070 * resolved along body-frame axes, averaged over time interval.
5071 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
5072 * resolved along body-frame axes, averaged over time interval.
5073 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
5074 * resolved along body-frame axes, averaged over time interval.
5075 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
5076 * resolved along body-frame axes, averaged over time interval.
5077 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
5078 * resolved along body-frame axes, averaged over time interval.
5079 * @param result instance where new estimated NED frame containing new body position,
5080 * velocity and coordinate transformation matrix will be stored.
5081 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
5082 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
5083 * body-to-NED-frame coordinate transformation matrix are
5084 * invalid.
5085 */
5086 public void navigate(
5087 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
5088 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
5089 final Acceleration fx, final Acceleration fy, final Acceleration fz,
5090 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
5091 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
5092 navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
5093 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
5094 }
5095
5096 /**
5097 * Runs precision local-navigation-frame inertial navigation equations.
5098 * NOTE: only the attitude update and specific force frame transformation
5099 * phases are precise.
5100 *
5101 * @param timeInterval time interval between epochs expressed in seconds (s).
5102 * @param oldPosition previous curvilinear position expressed in terms of latitude,
5103 * longitude and height.
5104 * @param oldC previous body-to-NED coordinate transformation.
5105 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
5106 * along north, east and down axes.
5107 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
5108 * resolved along body-frame axes, averaged over time interval.
5109 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
5110 * resolved along body-frame axes, averaged over time interval.
5111 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
5112 * resolved along body-frame axes, averaged over time interval.
5113 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
5114 * resolved along body-frame axes, averaged over time interval.
5115 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
5116 * resolved along body-frame axes, averaged over time interval.
5117 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
5118 * resolved along body-frame axes, averaged over time interval.
5119 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
5120 * @param result instance where new estimated NED frame containing new body position,
5121 * velocity and coordinate transformation matrix will be stored.
5122 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
5123 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
5124 * body-to-NED-frame coordinate transformation matrix are
5125 * invalid.
5126 */
5127 public void navigate(
5128 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
5129 final NEDVelocity oldVelocity, final Acceleration fx, final Acceleration fy, final Acceleration fz,
5130 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
5131 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
5132 InvalidSourceAndDestinationFrameTypeException {
5133 navigateNED(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
5134 accuracyThreshold, result);
5135 }
5136
5137 /**
5138 * Runs precision local-navigation-frame inertial navigation equations.
5139 * NOTE: only the attitude update and specific force frame transformation
5140 * phases are precise.
5141 *
5142 * @param timeInterval time interval between epochs expressed in seconds (s).
5143 * @param oldPosition previous curvilinear position expressed in terms of latitude,
5144 * longitude and height.
5145 * @param oldC previous body-to-NED coordinate transformation.
5146 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
5147 * along north, east and down axes.
5148 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
5149 * resolved along body-frame axes, averaged over time interval.
5150 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
5151 * resolved along body-frame axes, averaged over time interval.
5152 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
5153 * resolved along body-frame axes, averaged over time interval.
5154 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
5155 * resolved along body-frame axes, averaged over time interval.
5156 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
5157 * resolved along body-frame axes, averaged over time interval.
5158 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
5159 * resolved along body-frame axes, averaged over time interval.
5160 * @param result instance where new estimated NED frame containing new body position,
5161 * velocity and coordinate transformation matrix will be stored.
5162 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
5163 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
5164 * body-to-NED-frame coordinate transformation matrix are
5165 * invalid.
5166 */
5167 public void navigate(
5168 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
5169 final NEDVelocity oldVelocity, final Acceleration fx, final Acceleration fy, final Acceleration fz,
5170 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
5171 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
5172 navigate(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
5173 DEFAULT_ACCURACY_THRESHOLD, result);
5174 }
5175
5176 /**
5177 * Runs precision local-navigation-frame inertial navigation equations.
5178 * NOTE: only the attitude update and specific force frame transformation
5179 * phases are precise.
5180 *
5181 * @param timeInterval time interval between epochs.
5182 * @param oldPosition previous curvilinear position expressed in terms of latitude,
5183 * longitude and height.
5184 * @param oldC previous body-to-NED coordinate transformation.
5185 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
5186 * along north, east and down axes.
5187 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
5188 * resolved along body-frame axes, averaged over time interval.
5189 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
5190 * resolved along body-frame axes, averaged over time interval.
5191 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
5192 * resolved along body-frame axes, averaged over time interval.
5193 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
5194 * resolved along body-frame axes, averaged over time interval.
5195 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
5196 * resolved along body-frame axes, averaged over time interval.
5197 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
5198 * resolved along body-frame axes, averaged over time interval.
5199 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
5200 * @param result instance where new estimated NED frame containing new body position,
5201 * velocity and coordinate transformation matrix will be stored.
5202 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
5203 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
5204 * body-to-NED-frame coordinate transformation matrix are
5205 * invalid.
5206 */
5207 public void navigate(
5208 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
5209 final NEDVelocity oldVelocity, final Acceleration fx, final Acceleration fy, final Acceleration fz,
5210 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
5211 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
5212 InvalidSourceAndDestinationFrameTypeException {
5213 navigateNED(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
5214 accuracyThreshold, result);
5215 }
5216
5217 /**
5218 * Runs precision local-navigation-frame inertial navigation equations.
5219 * NOTE: only the attitude update and specific force frame transformation
5220 * phases are precise.
5221 *
5222 * @param timeInterval time interval between epochs.
5223 * @param oldPosition previous curvilinear position expressed in terms of latitude,
5224 * longitude and height.
5225 * @param oldC previous body-to-NED coordinate transformation.
5226 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
5227 * along north, east and down axes.
5228 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
5229 * resolved along body-frame axes, averaged over time interval.
5230 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
5231 * resolved along body-frame axes, averaged over time interval.
5232 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
5233 * resolved along body-frame axes, averaged over time interval.
5234 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
5235 * resolved along body-frame axes, averaged over time interval.
5236 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
5237 * resolved along body-frame axes, averaged over time interval.
5238 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
5239 * resolved along body-frame axes, averaged over time interval.
5240 * @param result instance where new estimated NED frame containing new body position,
5241 * velocity and coordinate transformation matrix will be stored.
5242 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
5243 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
5244 * body-to-NED-frame coordinate transformation matrix are
5245 * invalid.
5246 */
5247 public void navigate(
5248 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
5249 final NEDVelocity oldVelocity, final Acceleration fx, final Acceleration fy, final Acceleration fz,
5250 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
5251 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
5252 navigate(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
5253 DEFAULT_ACCURACY_THRESHOLD, result);
5254 }
5255
5256 /**
5257 * Runs precision local-navigation-frame inertial navigation equations.
5258 * NOTE: only the attitude update and specific force frame transformation
5259 * phases are precise.
5260 *
5261 * @param timeInterval time interval between epochs expressed in seconds (s).
5262 * @param oldLatitude previous latitude angle.
5263 * @param oldLongitude previous longitude angle.
5264 * @param oldHeight previous height.
5265 * @param oldC previous body-to-NED coordinate transformation.
5266 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
5267 * resolved along NED-frame axes and expressed in meters per second (m/s).
5268 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
5269 * resolved along NED-frame axes and expressed in meters per second (m/s).
5270 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
5271 * resolved along NED-frame axes and expressed in meters per second (m/s).
5272 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
5273 * resolved along body-frame axes, averaged over time interval.
5274 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
5275 * resolved along body-frame axes, averaged over time interval.
5276 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
5277 * resolved along body-frame axes, averaged over time interval.
5278 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
5279 * resolved along body-frame axes, averaged over time interval.
5280 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
5281 * resolved along body-frame axes, averaged over time interval.
5282 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
5283 * resolved along body-frame axes, averaged over time interval.
5284 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
5285 * @param result instance where new estimated NED frame containing new body position,
5286 * velocity and coordinate transformation matrix will be stored.
5287 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
5288 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
5289 * body-to-NED-frame coordinate transformation matrix are
5290 * invalid.
5291 */
5292 public void navigate(
5293 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
5294 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
5295 final Acceleration fx, final Acceleration fy, final Acceleration fz,
5296 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
5297 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
5298 InvalidSourceAndDestinationFrameTypeException {
5299 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
5300 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
5301 }
5302
5303 /**
5304 * Runs precision local-navigation-frame inertial navigation equations.
5305 * NOTE: only the attitude update and specific force frame transformation
5306 * phases are precise.
5307 *
5308 * @param timeInterval time interval between epochs expressed in seconds (s).
5309 * @param oldLatitude previous latitude angle.
5310 * @param oldLongitude previous longitude angle.
5311 * @param oldHeight previous height.
5312 * @param oldC previous body-to-NED coordinate transformation.
5313 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
5314 * resolved along NED-frame axes and expressed in meters per second (m/s).
5315 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
5316 * resolved along NED-frame axes and expressed in meters per second (m/s).
5317 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
5318 * resolved along NED-frame axes and expressed in meters per second (m/s).
5319 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
5320 * resolved along body-frame axes, averaged over time interval.
5321 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
5322 * resolved along body-frame axes, averaged over time interval.
5323 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
5324 * resolved along body-frame axes, averaged over time interval.
5325 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
5326 * resolved along body-frame axes, averaged over time interval.
5327 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
5328 * resolved along body-frame axes, averaged over time interval.
5329 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
5330 * resolved along body-frame axes, averaged over time interval.
5331 * @param result instance where new estimated NED frame containing new body position,
5332 * velocity and coordinate transformation matrix will be stored.
5333 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
5334 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
5335 * body-to-NED-frame coordinate transformation matrix are
5336 * invalid.
5337 */
5338 public void navigate(
5339 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
5340 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
5341 final Acceleration fx, final Acceleration fy, final Acceleration fz,
5342 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
5343 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
5344 navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
5345 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
5346 }
5347
5348 /**
5349 * Runs precision local-navigation-frame inertial navigation equations.
5350 * NOTE: only the attitude update and specific force frame transformation
5351 * phases are precise.
5352 *
5353 * @param timeInterval time interval between epochs.
5354 * @param oldLatitude previous latitude angle.
5355 * @param oldLongitude previous longitude angle.
5356 * @param oldHeight previous height.
5357 * @param oldC previous body-to-NED coordinate transformation.
5358 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
5359 * resolved along NED-frame axes and expressed in meters per second (m/s).
5360 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
5361 * resolved along NED-frame axes and expressed in meters per second (m/s).
5362 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
5363 * resolved along NED-frame axes and expressed in meters per second (m/s).
5364 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
5365 * resolved along body-frame axes, averaged over time interval.
5366 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
5367 * resolved along body-frame axes, averaged over time interval.
5368 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
5369 * resolved along body-frame axes, averaged over time interval.
5370 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
5371 * resolved along body-frame axes, averaged over time interval.
5372 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
5373 * resolved along body-frame axes, averaged over time interval.
5374 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
5375 * resolved along body-frame axes, averaged over time interval.
5376 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
5377 * @param result instance where new estimated NED frame containing new body position,
5378 * velocity and coordinate transformation matrix will be stored.
5379 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
5380 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
5381 * body-to-NED-frame coordinate transformation matrix are
5382 * invalid.
5383 */
5384 public void navigate(
5385 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
5386 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
5387 final Acceleration fx, final Acceleration fy, final Acceleration fz,
5388 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
5389 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
5390 InvalidSourceAndDestinationFrameTypeException {
5391 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
5392 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
5393 }
5394
5395 /**
5396 * Runs precision local-navigation-frame inertial navigation equations.
5397 * NOTE: only the attitude update and specific force frame transformation
5398 * phases are precise.
5399 *
5400 * @param timeInterval time interval between epochs.
5401 * @param oldLatitude previous latitude angle.
5402 * @param oldLongitude previous longitude angle.
5403 * @param oldHeight previous height.
5404 * @param oldC previous body-to-NED coordinate transformation.
5405 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
5406 * resolved along NED-frame axes and expressed in meters per second (m/s).
5407 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
5408 * resolved along NED-frame axes and expressed in meters per second (m/s).
5409 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
5410 * resolved along NED-frame axes and expressed in meters per second (m/s).
5411 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
5412 * resolved along body-frame axes, averaged over time interval.
5413 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
5414 * resolved along body-frame axes, averaged over time interval.
5415 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
5416 * resolved along body-frame axes, averaged over time interval.
5417 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
5418 * resolved along body-frame axes, averaged over time interval.
5419 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
5420 * resolved along body-frame axes, averaged over time interval.
5421 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
5422 * resolved along body-frame axes, averaged over time interval.
5423 * @param result instance where new estimated NED frame containing new body position,
5424 * velocity and coordinate transformation matrix will be stored.
5425 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
5426 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
5427 * body-to-NED-frame coordinate transformation matrix are
5428 * invalid.
5429 */
5430 public void navigate(
5431 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
5432 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
5433 final Acceleration fx, final Acceleration fy, final Acceleration fz,
5434 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
5435 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
5436 navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
5437 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
5438 }
5439
5440 /**
5441 * Runs precision local-navigation-frame inertial navigation equations.
5442 * NOTE: only the attitude update and specific force frame transformation
5443 * phases are precise.
5444 *
5445 * @param timeInterval time interval between epochs expressed in seconds (s).
5446 * @param oldPosition previous curvilinear position expressed in terms of latitude,
5447 * longitude and height.
5448 * @param oldC previous body-to-NED coordinate transformation.
5449 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
5450 * resolved along NED-frame axes and expressed in meters per second (m/s).
5451 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
5452 * resolved along NED-frame axes and expressed in meters per second (m/s).
5453 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
5454 * resolved along NED-frame axes and expressed in meters per second (m/s).
5455 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
5456 * resolved along body-frame axes, averaged over time interval.
5457 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
5458 * resolved along body-frame axes, averaged over time interval.
5459 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
5460 * resolved along body-frame axes, averaged over time interval.
5461 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
5462 * resolved along body-frame axes, averaged over time interval.
5463 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
5464 * resolved along body-frame axes, averaged over time interval.
5465 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
5466 * resolved along body-frame axes, averaged over time interval.
5467 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
5468 * @param result instance where new estimated NED frame containing new body position,
5469 * velocity and coordinate transformation matrix will be stored.
5470 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
5471 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
5472 * body-to-NED-frame coordinate transformation matrix are
5473 * invalid.
5474 */
5475 public void navigate(
5476 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
5477 final double oldVn, final double oldVe, final double oldVd,
5478 final Acceleration fx, final Acceleration fy, final Acceleration fz,
5479 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
5480 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
5481 InvalidSourceAndDestinationFrameTypeException {
5482 navigateNED(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
5483 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
5484 }
5485
5486 /**
5487 * Runs precision local-navigation-frame inertial navigation equations.
5488 * NOTE: only the attitude update and specific force frame transformation
5489 * phases are precise.
5490 *
5491 * @param timeInterval time interval between epochs expressed in seconds (s).
5492 * @param oldPosition previous curvilinear position expressed in terms of latitude,
5493 * longitude and height.
5494 * @param oldC previous body-to-NED coordinate transformation.
5495 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
5496 * resolved along NED-frame axes and expressed in meters per second (m/s).
5497 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
5498 * resolved along NED-frame axes and expressed in meters per second (m/s).
5499 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
5500 * resolved along NED-frame axes and expressed in meters per second (m/s).
5501 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
5502 * resolved along body-frame axes, averaged over time interval.
5503 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
5504 * resolved along body-frame axes, averaged over time interval.
5505 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
5506 * resolved along body-frame axes, averaged over time interval.
5507 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
5508 * resolved along body-frame axes, averaged over time interval.
5509 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
5510 * resolved along body-frame axes, averaged over time interval.
5511 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
5512 * resolved along body-frame axes, averaged over time interval.
5513 * @param result instance where new estimated NED frame containing new body position,
5514 * velocity and coordinate transformation matrix will be stored.
5515 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
5516 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
5517 * body-to-NED-frame coordinate transformation matrix are
5518 * invalid.
5519 */
5520 public void navigate(
5521 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
5522 final double oldVn, final double oldVe, final double oldVd,
5523 final Acceleration fx, final Acceleration fy, final Acceleration fz,
5524 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
5525 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
5526 navigate(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
5527 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
5528 }
5529
5530 /**
5531 * Runs precision local-navigation-frame inertial navigation equations.
5532 * NOTE: only the attitude update and specific force frame transformation
5533 * phases are precise.
5534 *
5535 * @param timeInterval time interval between epochs expressed in seconds (s).
5536 * @param oldPosition previous curvilinear position expressed in terms of latitude,
5537 * longitude and height.
5538 * @param oldC previous body-to-NED coordinate transformation.
5539 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
5540 * resolved along NED-frame axes and expressed in meters per second (m/s).
5541 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
5542 * resolved along NED-frame axes and expressed in meters per second (m/s).
5543 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
5544 * resolved along NED-frame axes and expressed in meters per second (m/s).
5545 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
5546 * resolved along body-frame axes, averaged over time interval.
5547 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
5548 * resolved along body-frame axes, averaged over time interval.
5549 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
5550 * resolved along body-frame axes, averaged over time interval.
5551 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
5552 * resolved along body-frame axes, averaged over time interval.
5553 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
5554 * resolved along body-frame axes, averaged over time interval.
5555 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
5556 * resolved along body-frame axes, averaged over time interval.
5557 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
5558 * @param result instance where new estimated NED frame containing new body position,
5559 * velocity and coordinate transformation matrix will be stored.
5560 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
5561 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
5562 * body-to-NED-frame coordinate transformation matrix are
5563 * invalid.
5564 */
5565 public void navigate(
5566 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
5567 final double oldVn, final double oldVe, final double oldVd,
5568 final Acceleration fx, final Acceleration fy, final Acceleration fz,
5569 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
5570 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
5571 InvalidSourceAndDestinationFrameTypeException {
5572 navigateNED(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
5573 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
5574 }
5575
5576 /**
5577 * Runs precision local-navigation-frame inertial navigation equations.
5578 * NOTE: only the attitude update and specific force frame transformation
5579 * phases are precise.
5580 *
5581 * @param timeInterval time interval between epochs expressed in seconds (s).
5582 * @param oldPosition previous curvilinear position expressed in terms of latitude,
5583 * longitude and height.
5584 * @param oldC previous body-to-NED coordinate transformation.
5585 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
5586 * resolved along NED-frame axes and expressed in meters per second (m/s).
5587 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
5588 * resolved along NED-frame axes and expressed in meters per second (m/s).
5589 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
5590 * resolved along NED-frame axes and expressed in meters per second (m/s).
5591 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
5592 * resolved along body-frame axes, averaged over time interval.
5593 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
5594 * resolved along body-frame axes, averaged over time interval.
5595 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
5596 * resolved along body-frame axes, averaged over time interval.
5597 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
5598 * resolved along body-frame axes, averaged over time interval.
5599 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
5600 * resolved along body-frame axes, averaged over time interval.
5601 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
5602 * resolved along body-frame axes, averaged over time interval.
5603 * @param result instance where new estimated NED frame containing new body position,
5604 * velocity and coordinate transformation matrix will be stored.
5605 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
5606 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
5607 * body-to-NED-frame coordinate transformation matrix are
5608 * invalid.
5609 */
5610 public void navigate(
5611 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
5612 final double oldVn, final double oldVe, final double oldVd,
5613 final Acceleration fx, final Acceleration fy, final Acceleration fz,
5614 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
5615 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
5616 navigate(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
5617 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
5618 }
5619
5620 /**
5621 * Runs precision local-navigation-frame inertial navigation equations.
5622 * NOTE: only the attitude update and specific force frame transformation
5623 * phases are precise.
5624 *
5625 * @param timeInterval time interval between epochs expressed in seconds (s).
5626 * @param oldLatitude previous latitude expressed in radians (rad).
5627 * @param oldLongitude previous longitude expressed in radians (rad).
5628 * @param oldHeight previous height expressed in meters (m).
5629 * @param oldC previous body-to-NED coordinate transformation.
5630 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
5631 * resolved along NED-frame axes and expressed in meters per second (m/s).
5632 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
5633 * resolved along NED-frame axes and expressed in meters per second (m/s).
5634 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
5635 * resolved along NED-frame axes and expressed in meters per second (m/s).
5636 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
5637 * resolved along body-frame axes, averaged over time interval.
5638 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
5639 * resolved along body-frame axes, averaged over time interval.
5640 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
5641 * resolved along body-frame axes, averaged over time interval.
5642 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
5643 * resolved along body-frame axes, averaged over time interval.
5644 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
5645 * resolved along body-frame axes, averaged over time interval.
5646 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
5647 * resolved along body-frame axes, averaged over time interval.
5648 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
5649 * @param result instance where new estimated NED frame containing new body position,
5650 * velocity and coordinate transformation matrix will be stored.
5651 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
5652 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
5653 * body-to-NED-frame coordinate transformation matrix are
5654 * invalid.
5655 */
5656 public void navigate(
5657 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
5658 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
5659 final Acceleration fx, final Acceleration fy, final Acceleration fz,
5660 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
5661 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
5662 InvalidSourceAndDestinationFrameTypeException {
5663 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
5664 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
5665 }
5666
5667 /**
5668 * Runs precision local-navigation-frame inertial navigation equations.
5669 * NOTE: only the attitude update and specific force frame transformation
5670 * phases are precise.
5671 *
5672 * @param timeInterval time interval between epochs expressed in seconds (s).
5673 * @param oldLatitude previous latitude expressed in radians (rad).
5674 * @param oldLongitude previous longitude expressed in radians (rad).
5675 * @param oldHeight previous height expressed in meters (m).
5676 * @param oldC previous body-to-NED coordinate transformation.
5677 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
5678 * resolved along NED-frame axes and expressed in meters per second (m/s).
5679 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
5680 * resolved along NED-frame axes and expressed in meters per second (m/s).
5681 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
5682 * resolved along NED-frame axes and expressed in meters per second (m/s).
5683 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
5684 * resolved along body-frame axes, averaged over time interval.
5685 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
5686 * resolved along body-frame axes, averaged over time interval.
5687 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
5688 * resolved along body-frame axes, averaged over time interval.
5689 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
5690 * resolved along body-frame axes, averaged over time interval.
5691 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
5692 * resolved along body-frame axes, averaged over time interval.
5693 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
5694 * resolved along body-frame axes, averaged over time interval.
5695 * @param result instance where new estimated NED frame containing new body position,
5696 * velocity and coordinate transformation matrix will be stored.
5697 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
5698 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
5699 * body-to-NED-frame coordinate transformation matrix are
5700 * invalid.
5701 */
5702 public void navigate(
5703 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
5704 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
5705 final Acceleration fx, final Acceleration fy, final Acceleration fz,
5706 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
5707 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
5708 navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
5709 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
5710 }
5711
5712 /**
5713 * Runs precision local-navigation-frame inertial navigation equations.
5714 * NOTE: only the attitude update and specific force frame transformation
5715 * phases are precise.
5716 *
5717 * @param timeInterval time interval between epochs.
5718 * @param oldLatitude previous latitude expressed in radians (rad).
5719 * @param oldLongitude previous longitude expressed in radians (rad).
5720 * @param oldHeight previous height expressed in meters (m).
5721 * @param oldC previous body-to-NED coordinate transformation.
5722 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
5723 * resolved along NED-frame axes and expressed in meters per second (m/s).
5724 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
5725 * resolved along NED-frame axes and expressed in meters per second (m/s).
5726 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
5727 * resolved along NED-frame axes and expressed in meters per second (m/s).
5728 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
5729 * resolved along body-frame axes, averaged over time interval.
5730 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
5731 * resolved along body-frame axes, averaged over time interval.
5732 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
5733 * resolved along body-frame axes, averaged over time interval.
5734 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
5735 * resolved along body-frame axes, averaged over time interval.
5736 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
5737 * resolved along body-frame axes, averaged over time interval.
5738 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
5739 * resolved along body-frame axes, averaged over time interval.
5740 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
5741 * @param result instance where new estimated NED frame containing new body position,
5742 * velocity and coordinate transformation matrix will be stored.
5743 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
5744 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
5745 * body-to-NED-frame coordinate transformation matrix are
5746 * invalid.
5747 */
5748 public void navigate(
5749 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
5750 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
5751 final Acceleration fx, final Acceleration fy, final Acceleration fz,
5752 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
5753 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
5754 InvalidSourceAndDestinationFrameTypeException {
5755 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
5756 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
5757 }
5758
5759 /**
5760 * Runs precision local-navigation-frame inertial navigation equations.
5761 * NOTE: only the attitude update and specific force frame transformation
5762 * phases are precise.
5763 *
5764 * @param timeInterval time interval between epochs.
5765 * @param oldLatitude previous latitude expressed in radians (rad).
5766 * @param oldLongitude previous longitude expressed in radians (rad).
5767 * @param oldHeight previous height expressed in meters (m).
5768 * @param oldC previous body-to-NED coordinate transformation.
5769 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
5770 * resolved along NED-frame axes and expressed in meters per second (m/s).
5771 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
5772 * resolved along NED-frame axes and expressed in meters per second (m/s).
5773 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
5774 * resolved along NED-frame axes and expressed in meters per second (m/s).
5775 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
5776 * resolved along body-frame axes, averaged over time interval.
5777 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
5778 * resolved along body-frame axes, averaged over time interval.
5779 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
5780 * resolved along body-frame axes, averaged over time interval.
5781 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
5782 * resolved along body-frame axes, averaged over time interval.
5783 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
5784 * resolved along body-frame axes, averaged over time interval.
5785 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
5786 * resolved along body-frame axes, averaged over time interval.
5787 * @param result instance where new estimated NED frame containing new body position,
5788 * velocity and coordinate transformation matrix will be stored.
5789 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
5790 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
5791 * body-to-NED-frame coordinate transformation matrix are
5792 * invalid.
5793 */
5794 public void navigate(
5795 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
5796 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
5797 final Acceleration fx, final Acceleration fy, final Acceleration fz,
5798 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
5799 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
5800 navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
5801 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
5802 }
5803
5804 /**
5805 * Runs precision local-navigation-frame inertial navigation equations.
5806 * NOTE: only the attitude update and specific force frame transformation
5807 * phases are precise.
5808 *
5809 * @param timeInterval time interval between epochs expressed in seconds (s).
5810 * @param oldLatitude previous latitude angle.
5811 * @param oldLongitude previous longitude angle.
5812 * @param oldHeight previous height.
5813 * @param oldC previous body-to-NED coordinate transformation.
5814 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
5815 * resolved along NED-frame axes.
5816 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
5817 * resolved along NED-frame axes.
5818 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
5819 * resolved along NED-frame axes.
5820 * @param kinematics body kinematics containing specific forces and angular rates applied to
5821 * the body.
5822 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
5823 * @param result instance where new estimated NED frame containing new body position,
5824 * velocity and coordinate transformation matrix will be stored.
5825 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
5826 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
5827 * body-to-NED-frame coordinate transformation matrix are
5828 * invalid.
5829 */
5830 public void navigate(
5831 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
5832 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
5833 final BodyKinematics kinematics, final double accuracyThreshold, final NEDFrame result)
5834 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
5835 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
5836 kinematics, accuracyThreshold, result);
5837 }
5838
5839 /**
5840 * Runs precision local-navigation-frame inertial navigation equations.
5841 * NOTE: only the attitude update and specific force frame transformation
5842 * phases are precise.
5843 *
5844 * @param timeInterval time interval between epochs expressed in seconds (s).
5845 * @param oldLatitude previous latitude angle.
5846 * @param oldLongitude previous longitude angle.
5847 * @param oldHeight previous height.
5848 * @param oldC previous body-to-NED coordinate transformation.
5849 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
5850 * resolved along NED-frame axes.
5851 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
5852 * resolved along NED-frame axes.
5853 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
5854 * resolved along NED-frame axes.
5855 * @param kinematics body kinematics containing specific forces and angular rates applied to
5856 * the body.
5857 * @param result instance where new estimated NED frame containing new body position,
5858 * velocity and coordinate transformation matrix will be stored.
5859 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
5860 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
5861 * body-to-NED-frame coordinate transformation matrix are
5862 * invalid.
5863 */
5864 public void navigate(
5865 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
5866 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
5867 final BodyKinematics kinematics, final NEDFrame result) throws InertialNavigatorException,
5868 InvalidSourceAndDestinationFrameTypeException {
5869 navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD, kinematics,
5870 DEFAULT_ACCURACY_THRESHOLD, result);
5871 }
5872
5873 /**
5874 * Runs precision local-navigation-frame inertial navigation equations.
5875 * NOTE: only the attitude update and specific force frame transformation
5876 * phases are precise.
5877 *
5878 * @param timeInterval time interval between epochs.
5879 * @param oldLatitude previous latitude angle.
5880 * @param oldLongitude previous longitude angle.
5881 * @param oldHeight previous height.
5882 * @param oldC previous body-to-NED coordinate transformation.
5883 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
5884 * resolved along NED-frame axes.
5885 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
5886 * resolved along NED-frame axes.
5887 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
5888 * resolved along NED-frame axes.
5889 * @param kinematics body kinematics containing specific forces and angular rates applied to
5890 * the body.
5891 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
5892 * @param result instance where new estimated NED frame containing new body position,
5893 * velocity and coordinate transformation matrix will be stored.
5894 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
5895 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
5896 * body-to-NED-frame coordinate transformation matrix are
5897 * invalid.
5898 */
5899 public void navigate(
5900 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
5901 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
5902 final BodyKinematics kinematics, final double accuracyThreshold, final NEDFrame result)
5903 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
5904 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
5905 kinematics, accuracyThreshold, result);
5906 }
5907
5908 /**
5909 * Runs precision local-navigation-frame inertial navigation equations.
5910 * NOTE: only the attitude update and specific force frame transformation
5911 * phases are precise.
5912 *
5913 * @param timeInterval time interval between epochs.
5914 * @param oldLatitude previous latitude angle.
5915 * @param oldLongitude previous longitude angle.
5916 * @param oldHeight previous height.
5917 * @param oldC previous body-to-NED coordinate transformation.
5918 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
5919 * resolved along NED-frame axes.
5920 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
5921 * resolved along NED-frame axes.
5922 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
5923 * resolved along NED-frame axes.
5924 * @param kinematics body kinematics containing specific forces and angular rates applied to
5925 * the body.
5926 * @param result instance where new estimated NED frame containing new body position,
5927 * velocity and coordinate transformation matrix will be stored.
5928 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
5929 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
5930 * body-to-NED-frame coordinate transformation matrix are
5931 * invalid.
5932 */
5933 public void navigate(
5934 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
5935 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
5936 final BodyKinematics kinematics, final NEDFrame result) throws InertialNavigatorException,
5937 InvalidSourceAndDestinationFrameTypeException {
5938 navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD, kinematics,
5939 DEFAULT_ACCURACY_THRESHOLD, result);
5940 }
5941
5942 /**
5943 * Runs precision local-navigation-frame inertial navigation equations.
5944 * NOTE: only the attitude update and specific force frame transformation
5945 * phases are precise.
5946 *
5947 * @param timeInterval time interval between epochs expressed in seconds (s).
5948 * @param oldFrame previous NED frame containing body position, velocity and
5949 * coordinate transformation matrix.
5950 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
5951 * resolved along body-frame axes, averaged over time interval and
5952 * expressed in meters per squared second (m/s^2).
5953 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
5954 * resolved along body-frame axes, averaged over time interval and
5955 * expressed in meters per squared second (m/s^2).
5956 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
5957 * resolved along body-frame axes, averaged over time interval and
5958 * expressed in meters per squared second (m/s^2).
5959 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
5960 * resolved along body-frame axes, averaged over time interval and
5961 * expressed in radians per second (rad/s).
5962 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
5963 * resolved along body-frame axes, averaged over time interval and
5964 * expressed in radians per second (rad/s).
5965 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
5966 * resolved along body-frame axes, averaged over time interval and
5967 * expressed in radians per second (rad/s).
5968 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
5969 * @param result instance where new estimated NED frame containing new body position,
5970 * velocity and coordinate transformation matrix will be stored.
5971 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
5972 */
5973 public void navigate(
5974 final double timeInterval, final NEDFrame oldFrame, final double fx, final double fy, final double fz,
5975 final double angularRateX, final double angularRateY, final double angularRateZ,
5976 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException {
5977 navigateNED(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold,
5978 result);
5979 }
5980
5981 /**
5982 * Runs precision local-navigation-frame inertial navigation equations.
5983 * NOTE: only the attitude update and specific force frame transformation
5984 * phases are precise.
5985 *
5986 * @param timeInterval time interval between epochs expressed in seconds (s).
5987 * @param oldFrame previous NED frame containing body position, velocity and
5988 * coordinate transformation matrix.
5989 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
5990 * resolved along body-frame axes, averaged over time interval and
5991 * expressed in meters per squared second (m/s^2).
5992 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
5993 * resolved along body-frame axes, averaged over time interval and
5994 * expressed in meters per squared second (m/s^2).
5995 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
5996 * resolved along body-frame axes, averaged over time interval and
5997 * expressed in meters per squared second (m/s^2).
5998 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
5999 * resolved along body-frame axes, averaged over time interval and
6000 * expressed in radians per second (rad/s).
6001 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
6002 * resolved along body-frame axes, averaged over time interval and
6003 * expressed in radians per second (rad/s).
6004 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
6005 * resolved along body-frame axes, averaged over time interval and
6006 * expressed in radians per second (rad/s).
6007 * @param result instance where new estimated NED frame containing new body position,
6008 * velocity and coordinate transformation matrix will be stored.
6009 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
6010 */
6011 public void navigate(
6012 final double timeInterval, final NEDFrame oldFrame, final double fx, final double fy, final double fz,
6013 final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result)
6014 throws InertialNavigatorException {
6015 navigate(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
6016 DEFAULT_ACCURACY_THRESHOLD, result);
6017 }
6018
6019 /**
6020 * Runs precision local-navigation-frame inertial navigation equations.
6021 * NOTE: only the attitude update and specific force frame transformation
6022 * phases are precise.
6023 *
6024 * @param timeInterval time interval between epochs.
6025 * @param oldFrame previous NED frame containing body position, velocity and
6026 * coordinate transformation matrix.
6027 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
6028 * resolved along body-frame axes, averaged over time interval and
6029 * expressed in meters per squared second (m/s^2).
6030 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
6031 * resolved along body-frame axes, averaged over time interval and
6032 * expressed in meters per squared second (m/s^2).
6033 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
6034 * resolved along body-frame axes, averaged over time interval and
6035 * expressed in meters per squared second (m/s^2).
6036 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
6037 * resolved along body-frame axes, averaged over time interval and
6038 * expressed in radians per second (rad/s).
6039 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
6040 * resolved along body-frame axes, averaged over time interval and
6041 * expressed in radians per second (rad/s).
6042 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
6043 * resolved along body-frame axes, averaged over time interval and
6044 * expressed in radians per second (rad/s).
6045 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
6046 * @param result instance where new estimated NED frame containing new body position,
6047 * velocity and coordinate transformation matrix will be stored.
6048 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
6049 */
6050 public void navigate(
6051 final Time timeInterval, final NEDFrame oldFrame, final double fx, final double fy, final double fz,
6052 final double angularRateX, final double angularRateY, final double angularRateZ,
6053 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException {
6054 navigateNED(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold,
6055 result);
6056 }
6057
6058 /**
6059 * Runs precision local-navigation-frame inertial navigation equations.
6060 * NOTE: only the attitude update and specific force frame transformation
6061 * phases are precise.
6062 *
6063 * @param timeInterval time interval between epochs.
6064 * @param oldFrame previous NED frame containing body position, velocity and
6065 * coordinate transformation matrix.
6066 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
6067 * resolved along body-frame axes, averaged over time interval and
6068 * expressed in meters per squared second (m/s^2).
6069 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
6070 * resolved along body-frame axes, averaged over time interval and
6071 * expressed in meters per squared second (m/s^2).
6072 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
6073 * resolved along body-frame axes, averaged over time interval and
6074 * expressed in meters per squared second (m/s^2).
6075 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
6076 * resolved along body-frame axes, averaged over time interval and
6077 * expressed in radians per second (rad/s).
6078 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
6079 * resolved along body-frame axes, averaged over time interval and
6080 * expressed in radians per second (rad/s).
6081 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
6082 * resolved along body-frame axes, averaged over time interval and
6083 * expressed in radians per second (rad/s).
6084 * @param result instance where new estimated NED frame containing new body position,
6085 * velocity and coordinate transformation matrix will be stored.
6086 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
6087 */
6088 public void navigate(
6089 final Time timeInterval, final NEDFrame oldFrame, final double fx, final double fy, final double fz,
6090 final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result)
6091 throws InertialNavigatorException {
6092 navigate(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
6093 DEFAULT_ACCURACY_THRESHOLD, result);
6094 }
6095
6096 /**
6097 * Runs precision local-navigation-frame inertial navigation equations.
6098 * NOTE: only the attitude update and specific force frame transformation
6099 * phases are precise.
6100 *
6101 * @param timeInterval time interval between epochs expressed in seconds (s).
6102 * @param oldFrame previous NED frame containing body position, velocity and
6103 * coordinate transformation matrix.
6104 * @param kinematics body kinematics containing specific forces and angular rates applied to
6105 * the body.
6106 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
6107 * @param result instance where new estimated NED frame containing new body position,
6108 * velocity and coordinate transformation matrix will be stored.
6109 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
6110 */
6111 public void navigate(
6112 final double timeInterval, final NEDFrame oldFrame, final BodyKinematics kinematics,
6113 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException {
6114 navigateNED(timeInterval, oldFrame, kinematics, accuracyThreshold, result);
6115 }
6116
6117 /**
6118 * Runs precision local-navigation-frame inertial navigation equations.
6119 * NOTE: only the attitude update and specific force frame transformation
6120 * phases are precise.
6121 *
6122 * @param timeInterval time interval between epochs expressed in seconds (s).
6123 * @param oldFrame previous NED frame containing body position, velocity and
6124 * coordinate transformation matrix.
6125 * @param kinematics body kinematics containing specific forces and angular rates applied to
6126 * the body.
6127 * @param result instance where new estimated NED frame containing new body position,
6128 * velocity and coordinate transformation matrix will be stored.
6129 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
6130 */
6131 public void navigate(
6132 final double timeInterval, final NEDFrame oldFrame, final BodyKinematics kinematics, final NEDFrame result)
6133 throws InertialNavigatorException {
6134 navigate(timeInterval, oldFrame, kinematics, DEFAULT_ACCURACY_THRESHOLD, result);
6135 }
6136
6137 /**
6138 * Runs precision local-navigation-frame inertial navigation equations.
6139 * NOTE: only the attitude update and specific force frame transformation
6140 * phases are precise.
6141 *
6142 * @param timeInterval time interval between epochs.
6143 * @param oldFrame previous NED frame containing body position, velocity and
6144 * coordinate transformation matrix.
6145 * @param kinematics body kinematics containing specific forces and angular rates applied to
6146 * the body.
6147 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
6148 * @param result instance where new estimated NED frame containing new body position,
6149 * velocity and coordinate transformation matrix will be stored.
6150 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
6151 */
6152 public void navigate(
6153 final Time timeInterval, final NEDFrame oldFrame, final BodyKinematics kinematics,
6154 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException {
6155 navigateNED(timeInterval, oldFrame, kinematics, accuracyThreshold, result);
6156 }
6157
6158 /**
6159 * Runs precision local-navigation-frame inertial navigation equations.
6160 * NOTE: only the attitude update and specific force frame transformation
6161 * phases are precise.
6162 *
6163 * @param timeInterval time interval between epochs.
6164 * @param oldFrame previous NED frame containing body position, velocity and
6165 * coordinate transformation matrix.
6166 * @param kinematics body kinematics containing specific forces and angular rates applied to
6167 * the body.
6168 * @param result instance where new estimated NED frame containing new body position,
6169 * velocity and coordinate transformation matrix will be stored.
6170 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
6171 */
6172 public void navigate(
6173 final Time timeInterval, final NEDFrame oldFrame, final BodyKinematics kinematics, final NEDFrame result)
6174 throws InertialNavigatorException {
6175 navigate(timeInterval, oldFrame, kinematics, DEFAULT_ACCURACY_THRESHOLD, result);
6176 }
6177
6178 /**
6179 * Runs precision local-navigation-frame inertial navigation equations.
6180 * NOTE: only the attitude update and specific force frame transformation
6181 * phases are precise.
6182 *
6183 * @param timeInterval time interval between epochs expressed in seconds (s).
6184 * @param oldFrame previous NED frame containing body position, velocity and
6185 * coordinate transformation matrix.
6186 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
6187 * resolved along body-frame axes, averaged over time interval.
6188 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
6189 * resolved along body-frame axes, averaged over time interval.
6190 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
6191 * resolved along body-frame axes, averaged over time interval.
6192 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
6193 * resolved along body-frame axes, averaged over time interval and
6194 * expressed in radians per second (rad/s).
6195 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
6196 * resolved along body-frame axes, averaged over time interval and
6197 * expressed in radians per second (rad/s).
6198 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
6199 * resolved along body-frame axes, averaged over time interval and
6200 * expressed in radians per second (rad/s).
6201 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
6202 * @param result instance where new estimated NED frame containing new body position,
6203 * velocity and coordinate transformation matrix will be stored.
6204 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
6205 */
6206 public void navigate(
6207 final double timeInterval, final NEDFrame oldFrame,
6208 final Acceleration fx, final Acceleration fy, final Acceleration fz,
6209 final double angularRateX, final double angularRateY, final double angularRateZ,
6210 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException {
6211 navigateNED(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold,
6212 result);
6213 }
6214
6215 /**
6216 * Runs precision local-navigation-frame inertial navigation equations.
6217 * NOTE: only the attitude update and specific force frame transformation
6218 * phases are precise.
6219 *
6220 * @param timeInterval time interval between epochs expressed in seconds (s).
6221 * @param oldFrame previous NED frame containing body position, velocity and
6222 * coordinate transformation matrix.
6223 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
6224 * resolved along body-frame axes, averaged over time interval.
6225 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
6226 * resolved along body-frame axes, averaged over time interval.
6227 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
6228 * resolved along body-frame axes, averaged over time interval.
6229 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
6230 * resolved along body-frame axes, averaged over time interval and
6231 * expressed in radians per second (rad/s).
6232 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
6233 * resolved along body-frame axes, averaged over time interval and
6234 * expressed in radians per second (rad/s).
6235 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
6236 * resolved along body-frame axes, averaged over time interval and
6237 * expressed in radians per second (rad/s).
6238 * @param result instance where new estimated NED frame containing new body position,
6239 * velocity and coordinate transformation matrix will be stored.
6240 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
6241 */
6242 public void navigate(
6243 final double timeInterval, final NEDFrame oldFrame,
6244 final Acceleration fx, final Acceleration fy, final Acceleration fz,
6245 final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result)
6246 throws InertialNavigatorException {
6247 navigate(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
6248 DEFAULT_ACCURACY_THRESHOLD, result);
6249 }
6250
6251 /**
6252 * Runs precision local-navigation-frame inertial navigation equations.
6253 * NOTE: only the attitude update and specific force frame transformation
6254 * phases are precise.
6255 *
6256 * @param timeInterval time interval between epochs.
6257 * @param oldFrame previous NED frame containing body position, velocity and
6258 * coordinate transformation matrix.
6259 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
6260 * resolved along body-frame axes, averaged over time interval.
6261 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
6262 * resolved along body-frame axes, averaged over time interval.
6263 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
6264 * resolved along body-frame axes, averaged over time interval.
6265 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
6266 * resolved along body-frame axes, averaged over time interval and
6267 * expressed in radians per second (rad/s).
6268 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
6269 * resolved along body-frame axes, averaged over time interval and
6270 * expressed in radians per second (rad/s).
6271 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
6272 * resolved along body-frame axes, averaged over time interval and
6273 * expressed in radians per second (rad/s).
6274 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
6275 * @param result instance where new estimated NED frame containing new body position,
6276 * velocity and coordinate transformation matrix will be stored.
6277 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
6278 */
6279 public void navigate(
6280 final Time timeInterval, final NEDFrame oldFrame,
6281 final Acceleration fx, final Acceleration fy, final Acceleration fz,
6282 final double angularRateX, final double angularRateY, final double angularRateZ,
6283 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException {
6284 navigateNED(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold,
6285 result);
6286 }
6287
6288 /**
6289 * Runs precision local-navigation-frame inertial navigation equations.
6290 * NOTE: only the attitude update and specific force frame transformation
6291 * phases are precise.
6292 *
6293 * @param timeInterval time interval between epochs.
6294 * @param oldFrame previous NED frame containing body position, velocity and
6295 * coordinate transformation matrix.
6296 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
6297 * resolved along body-frame axes, averaged over time interval.
6298 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
6299 * resolved along body-frame axes, averaged over time interval.
6300 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
6301 * resolved along body-frame axes, averaged over time interval.
6302 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
6303 * resolved along body-frame axes, averaged over time interval and
6304 * expressed in radians per second (rad/s).
6305 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
6306 * resolved along body-frame axes, averaged over time interval and
6307 * expressed in radians per second (rad/s).
6308 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
6309 * resolved along body-frame axes, averaged over time interval and
6310 * expressed in radians per second (rad/s).
6311 * @param result instance where new estimated NED frame containing new body position,
6312 * velocity and coordinate transformation matrix will be stored.
6313 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
6314 */
6315 public void navigate(
6316 final Time timeInterval, final NEDFrame oldFrame,
6317 final Acceleration fx, final Acceleration fy, final Acceleration fz,
6318 final double angularRateX, final double angularRateY, final double angularRateZ,
6319 final NEDFrame result) throws InertialNavigatorException {
6320 navigateNED(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
6321 DEFAULT_ACCURACY_THRESHOLD, result);
6322 }
6323
6324 /**
6325 * Runs precision local-navigation-frame inertial navigation equations.
6326 * NOTE: only the attitude update and specific force frame transformation
6327 * phases are precise.
6328 *
6329 * @param timeInterval time interval between epochs expressed in seconds (s).
6330 * @param oldFrame previous NED frame containing body position, velocity and
6331 * coordinate transformation matrix.
6332 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
6333 * resolved along body-frame axes, averaged over time interval and
6334 * expressed in meters per squared second (m/s^2).
6335 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
6336 * resolved along body-frame axes, averaged over time interval and
6337 * expressed in meters per squared second (m/s^2).
6338 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
6339 * resolved along body-frame axes, averaged over time interval and
6340 * expressed in meters per squared second (m/s^2).
6341 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
6342 * resolved along body-frame axes, averaged over time interval.
6343 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
6344 * resolved along body-frame axes, averaged over time interval.
6345 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
6346 * resolved along body-frame axes, averaged over time interval.
6347 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
6348 * @param result instance where new estimated NED frame containing new body position,
6349 * velocity and coordinate transformation matrix will be stored.
6350 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
6351 */
6352 public void navigate(
6353 final double timeInterval, final NEDFrame oldFrame,
6354 final double fx, final double fy, final double fz,
6355 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
6356 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException {
6357 navigateNED(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold,
6358 result);
6359 }
6360
6361 /**
6362 * Runs precision local-navigation-frame inertial navigation equations.
6363 * NOTE: only the attitude update and specific force frame transformation
6364 * phases are precise.
6365 *
6366 * @param timeInterval time interval between epochs expressed in seconds (s).
6367 * @param oldFrame previous NED frame containing body position, velocity and
6368 * coordinate transformation matrix.
6369 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
6370 * resolved along body-frame axes, averaged over time interval and
6371 * expressed in meters per squared second (m/s^2).
6372 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
6373 * resolved along body-frame axes, averaged over time interval and
6374 * expressed in meters per squared second (m/s^2).
6375 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
6376 * resolved along body-frame axes, averaged over time interval and
6377 * expressed in meters per squared second (m/s^2).
6378 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
6379 * resolved along body-frame axes, averaged over time interval.
6380 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
6381 * resolved along body-frame axes, averaged over time interval.
6382 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
6383 * resolved along body-frame axes, averaged over time interval.
6384 * @param result instance where new estimated NED frame containing new body position,
6385 * velocity and coordinate transformation matrix will be stored.
6386 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
6387 */
6388 public void navigate(
6389 final double timeInterval, final NEDFrame oldFrame,
6390 final double fx, final double fy, final double fz,
6391 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
6392 final NEDFrame result) throws InertialNavigatorException {
6393 navigate(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
6394 DEFAULT_ACCURACY_THRESHOLD, result);
6395 }
6396
6397 /**
6398 * Runs precision local-navigation-frame inertial navigation equations.
6399 * NOTE: only the attitude update and specific force frame transformation
6400 * phases are precise.
6401 *
6402 * @param timeInterval time interval between epochs.
6403 * @param oldFrame previous NED frame containing body position, velocity and
6404 * coordinate transformation matrix.
6405 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
6406 * resolved along body-frame axes, averaged over time interval and
6407 * expressed in meters per squared second (m/s^2).
6408 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
6409 * resolved along body-frame axes, averaged over time interval and
6410 * expressed in meters per squared second (m/s^2).
6411 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
6412 * resolved along body-frame axes, averaged over time interval and
6413 * expressed in meters per squared second (m/s^2).
6414 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
6415 * resolved along body-frame axes, averaged over time interval.
6416 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
6417 * resolved along body-frame axes, averaged over time interval.
6418 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
6419 * resolved along body-frame axes, averaged over time interval.
6420 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
6421 * @param result instance where new estimated NED frame containing new body position,
6422 * velocity and coordinate transformation matrix will be stored.
6423 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
6424 */
6425 public void navigate(
6426 final Time timeInterval, final NEDFrame oldFrame, final double fx, final double fy, final double fz,
6427 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
6428 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException {
6429 navigateNED(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold,
6430 result);
6431 }
6432
6433 /**
6434 * Runs precision local-navigation-frame inertial navigation equations.
6435 * NOTE: only the attitude update and specific force frame transformation
6436 * phases are precise.
6437 *
6438 * @param timeInterval time interval between epochs.
6439 * @param oldFrame previous NED frame containing body position, velocity and
6440 * coordinate transformation matrix.
6441 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
6442 * resolved along body-frame axes, averaged over time interval and
6443 * expressed in meters per squared second (m/s^2).
6444 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
6445 * resolved along body-frame axes, averaged over time interval and
6446 * expressed in meters per squared second (m/s^2).
6447 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
6448 * resolved along body-frame axes, averaged over time interval and
6449 * expressed in meters per squared second (m/s^2).
6450 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
6451 * resolved along body-frame axes, averaged over time interval.
6452 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
6453 * resolved along body-frame axes, averaged over time interval.
6454 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
6455 * resolved along body-frame axes, averaged over time interval.
6456 * @param result instance where new estimated NED frame containing new body position,
6457 * velocity and coordinate transformation matrix will be stored.
6458 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
6459 */
6460 public void navigate(
6461 final Time timeInterval, final NEDFrame oldFrame, final double fx, final double fy, final double fz,
6462 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
6463 final NEDFrame result) throws InertialNavigatorException {
6464 navigate(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
6465 DEFAULT_ACCURACY_THRESHOLD, result);
6466 }
6467
6468 /**
6469 * Runs precision local-navigation-frame inertial navigation equations.
6470 * NOTE: only the attitude update and specific force frame transformation
6471 * phases are precise.
6472 *
6473 * @param timeInterval time interval between epochs expressed in seconds (s).
6474 * @param oldFrame previous NED frame containing body position, velocity and
6475 * coordinate transformation matrix.
6476 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
6477 * resolved along body-frame axes, averaged over time interval and
6478 * expressed in meters per squared second (m/s^2).
6479 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
6480 * resolved along body-frame axes, averaged over time interval and
6481 * expressed in meters per squared second (m/s^2).
6482 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
6483 * resolved along body-frame axes, averaged over time interval and
6484 * expressed in meters per squared second (m/s^2).
6485 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
6486 * resolved along body-frame axes, averaged over time interval.
6487 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
6488 * resolved along body-frame axes, averaged over time interval.
6489 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
6490 * resolved along body-frame axes, averaged over time interval.
6491 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
6492 * @param result instance where new estimated NED frame containing new body position,
6493 * velocity and coordinate transformation matrix will be stored.
6494 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
6495 */
6496 public void navigate(
6497 final double timeInterval, final NEDFrame oldFrame,
6498 final Acceleration fx, final Acceleration fy, final Acceleration fz,
6499 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
6500 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException {
6501 navigateNED(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold,
6502 result);
6503 }
6504
6505 /**
6506 * Runs precision local-navigation-frame inertial navigation equations.
6507 * NOTE: only the attitude update and specific force frame transformation
6508 * phases are precise.
6509 *
6510 * @param timeInterval time interval between epochs expressed in seconds (s).
6511 * @param oldFrame previous NED frame containing body position, velocity and
6512 * coordinate transformation matrix.
6513 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
6514 * resolved along body-frame axes, averaged over time interval and
6515 * expressed in meters per squared second (m/s^2).
6516 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
6517 * resolved along body-frame axes, averaged over time interval and
6518 * expressed in meters per squared second (m/s^2).
6519 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
6520 * resolved along body-frame axes, averaged over time interval and
6521 * expressed in meters per squared second (m/s^2).
6522 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
6523 * resolved along body-frame axes, averaged over time interval.
6524 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
6525 * resolved along body-frame axes, averaged over time interval.
6526 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
6527 * resolved along body-frame axes, averaged over time interval.
6528 * @param result instance where new estimated NED frame containing new body position,
6529 * velocity and coordinate transformation matrix will be stored.
6530 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
6531 */
6532 public void navigate(
6533 final double timeInterval, final NEDFrame oldFrame,
6534 final Acceleration fx, final Acceleration fy, final Acceleration fz,
6535 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
6536 final NEDFrame result) throws InertialNavigatorException {
6537 navigate(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
6538 DEFAULT_ACCURACY_THRESHOLD, result);
6539 }
6540
6541 /**
6542 * Runs precision local-navigation-frame inertial navigation equations.
6543 * NOTE: only the attitude update and specific force frame transformation
6544 * phases are precise.
6545 *
6546 * @param timeInterval time interval between epochs expressed in seconds (s).
6547 * @param oldFrame previous NED frame containing body position, velocity and
6548 * coordinate transformation matrix.
6549 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
6550 * resolved along body-frame axes, averaged over time interval and
6551 * expressed in meters per squared second (m/s^2).
6552 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
6553 * resolved along body-frame axes, averaged over time interval and
6554 * expressed in meters per squared second (m/s^2).
6555 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
6556 * resolved along body-frame axes, averaged over time interval and
6557 * expressed in meters per squared second (m/s^2).
6558 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
6559 * resolved along body-frame axes, averaged over time interval.
6560 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
6561 * resolved along body-frame axes, averaged over time interval.
6562 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
6563 * resolved along body-frame axes, averaged over time interval.
6564 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
6565 * @param result instance where new estimated NED frame containing new body position,
6566 * velocity and coordinate transformation matrix will be stored.
6567 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
6568 */
6569 public void navigate(
6570 final Time timeInterval, final NEDFrame oldFrame,
6571 final Acceleration fx, final Acceleration fy, final Acceleration fz,
6572 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
6573 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException {
6574 navigateNED(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold,
6575 result);
6576 }
6577
6578 /**
6579 * Runs precision local-navigation-frame inertial navigation equations.
6580 * NOTE: only the attitude update and specific force frame transformation
6581 * phases are precise.
6582 *
6583 * @param timeInterval time interval between epochs expressed in seconds (s).
6584 * @param oldFrame previous NED frame containing body position, velocity and
6585 * coordinate transformation matrix.
6586 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
6587 * resolved along body-frame axes, averaged over time interval and
6588 * expressed in meters per squared second (m/s^2).
6589 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
6590 * resolved along body-frame axes, averaged over time interval and
6591 * expressed in meters per squared second (m/s^2).
6592 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
6593 * resolved along body-frame axes, averaged over time interval and
6594 * expressed in meters per squared second (m/s^2).
6595 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
6596 * resolved along body-frame axes, averaged over time interval.
6597 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
6598 * resolved along body-frame axes, averaged over time interval.
6599 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
6600 * resolved along body-frame axes, averaged over time interval.
6601 * @param result instance where new estimated NED frame containing new body position,
6602 * velocity and coordinate transformation matrix will be stored.
6603 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
6604 */
6605 public void navigate(
6606 final Time timeInterval, final NEDFrame oldFrame,
6607 final Acceleration fx, final Acceleration fy, final Acceleration fz,
6608 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
6609 final NEDFrame result) throws InertialNavigatorException {
6610 navigate(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
6611 DEFAULT_ACCURACY_THRESHOLD, result);
6612 }
6613
6614 /**
6615 * Runs precision local-navigation-frame inertial navigation equations.
6616 * NOTE: only the attitude update and specific force frame transformation
6617 * phases are precise.
6618 *
6619 * @param timeInterval time interval between epochs expressed in seconds (s).
6620 * @param oldLatitude previous latitude expressed in radians (rad).
6621 * @param oldLongitude previous longitude expressed in radians (rad).
6622 * @param oldHeight previous height expressed in meters (m).
6623 * @param oldC previous body-to-NED coordinate transformation.
6624 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
6625 * resolved along NED-frame axes and expressed in meters per second (m/s).
6626 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
6627 * resolved along NED-frame axes and expressed in meters per second (m/s).
6628 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
6629 * resolved along NED-frame axes and expressed in meters per second (m/s).
6630 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
6631 * resolved along body-frame axes, averaged over time interval and
6632 * expressed in meters per squared second (m/s^2).
6633 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
6634 * resolved along body-frame axes, averaged over time interval and
6635 * expressed in meters per squared second (m/s^2).
6636 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
6637 * resolved along body-frame axes, averaged over time interval and
6638 * expressed in meters per squared second (m/s^2).
6639 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
6640 * resolved along body-frame axes, averaged over time interval and
6641 * expressed in radians per second (rad/s).
6642 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
6643 * resolved along body-frame axes, averaged over time interval and
6644 * expressed in radians per second (rad/s).
6645 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
6646 * resolved along body-frame axes, averaged over time interval and
6647 * expressed in radians per second (rad/s).
6648 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
6649 * @return estimated NED frame containing new body position, velocity and coordinate
6650 * transformation matrix.
6651 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
6652 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
6653 * body-to-NED-frame coordinate transformation matrix are
6654 * invalid.
6655 */
6656 public NEDFrame navigateAndReturnNew(
6657 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
6658 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
6659 final double fx, final double fy, final double fz,
6660 final double angularRateX, final double angularRateY, final double angularRateZ,
6661 final double accuracyThreshold) throws InertialNavigatorException,
6662 InvalidSourceAndDestinationFrameTypeException {
6663 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
6664 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold);
6665 }
6666
6667 /**
6668 * Runs precision local-navigation-frame inertial navigation equations.
6669 * NOTE: only the attitude update and specific force frame transformation
6670 * phases are precise.
6671 *
6672 * @param timeInterval time interval between epochs expressed in seconds (s).
6673 * @param oldLatitude previous latitude expressed in radians (rad).
6674 * @param oldLongitude previous longitude expressed in radians (rad).
6675 * @param oldHeight previous height expressed in meters (m).
6676 * @param oldC previous body-to-NED coordinate transformation.
6677 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
6678 * resolved along NED-frame axes and expressed in meters per second (m/s).
6679 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
6680 * resolved along NED-frame axes and expressed in meters per second (m/s).
6681 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
6682 * resolved along NED-frame axes and expressed in meters per second (m/s).
6683 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
6684 * resolved along body-frame axes, averaged over time interval and
6685 * expressed in meters per squared second (m/s^2).
6686 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
6687 * resolved along body-frame axes, averaged over time interval and
6688 * expressed in meters per squared second (m/s^2).
6689 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
6690 * resolved along body-frame axes, averaged over time interval and
6691 * expressed in meters per squared second (m/s^2).
6692 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
6693 * resolved along body-frame axes, averaged over time interval and
6694 * expressed in radians per second (rad/s).
6695 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
6696 * resolved along body-frame axes, averaged over time interval and
6697 * expressed in radians per second (rad/s).
6698 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
6699 * resolved along body-frame axes, averaged over time interval and
6700 * expressed in radians per second (rad/s).
6701 * @return estimated NED frame containing new body position, velocity and coordinate
6702 * transformation matrix.
6703 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
6704 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
6705 * body-to-NED-frame coordinate transformation matrix are
6706 * invalid.
6707 */
6708 public NEDFrame navigateAndReturnNew(
6709 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
6710 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
6711 final double fx, final double fy, final double fz,
6712 final double angularRateX, final double angularRateY, final double angularRateZ)
6713 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
6714 return navigateAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
6715 fx, fy, fz, angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
6716 }
6717
6718 /**
6719 * Runs precision local-navigation-frame inertial navigation equations.
6720 * NOTE: only the attitude update and specific force frame transformation
6721 * phases are precise.
6722 *
6723 * @param timeInterval time interval between epochs.
6724 * @param oldLatitude previous latitude expressed in radians (rad).
6725 * @param oldLongitude previous longitude expressed in radians (rad).
6726 * @param oldHeight previous height expressed in meters (m).
6727 * @param oldC previous body-to-NED coordinate transformation.
6728 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
6729 * resolved along NED-frame axes and expressed in meters per second (m/s).
6730 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
6731 * resolved along NED-frame axes and expressed in meters per second (m/s).
6732 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
6733 * resolved along NED-frame axes and expressed in meters per second (m/s).
6734 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
6735 * resolved along body-frame axes, averaged over time interval and
6736 * expressed in meters per squared second (m/s^2).
6737 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
6738 * resolved along body-frame axes, averaged over time interval and
6739 * expressed in meters per squared second (m/s^2).
6740 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
6741 * resolved along body-frame axes, averaged over time interval and
6742 * expressed in meters per squared second (m/s^2).
6743 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
6744 * resolved along body-frame axes, averaged over time interval and
6745 * expressed in radians per second (rad/s).
6746 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
6747 * resolved along body-frame axes, averaged over time interval and
6748 * expressed in radians per second (rad/s).
6749 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
6750 * resolved along body-frame axes, averaged over time interval and
6751 * expressed in radians per second (rad/s).
6752 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
6753 * @return estimated NED frame containing new body position, velocity and coordinate
6754 * transformation matrix.
6755 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
6756 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
6757 * body-to-NED-frame coordinate transformation matrix are
6758 * invalid.
6759 */
6760 public NEDFrame navigateAndReturnNew(
6761 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
6762 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
6763 final double fx, final double fy, final double fz,
6764 final double angularRateX, final double angularRateY, final double angularRateZ,
6765 final double accuracyThreshold) throws InertialNavigatorException,
6766 InvalidSourceAndDestinationFrameTypeException {
6767 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
6768 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold);
6769 }
6770
6771 /**
6772 * Runs precision local-navigation-frame inertial navigation equations.
6773 * NOTE: only the attitude update and specific force frame transformation
6774 * phases are precise.
6775 *
6776 * @param timeInterval time interval between epochs.
6777 * @param oldLatitude previous latitude expressed in radians (rad).
6778 * @param oldLongitude previous longitude expressed in radians (rad).
6779 * @param oldHeight previous height expressed in meters (m).
6780 * @param oldC previous body-to-NED coordinate transformation.
6781 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
6782 * resolved along NED-frame axes and expressed in meters per second (m/s).
6783 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
6784 * resolved along NED-frame axes and expressed in meters per second (m/s).
6785 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
6786 * resolved along NED-frame axes and expressed in meters per second (m/s).
6787 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
6788 * resolved along body-frame axes, averaged over time interval and
6789 * expressed in meters per squared second (m/s^2).
6790 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
6791 * resolved along body-frame axes, averaged over time interval and
6792 * expressed in meters per squared second (m/s^2).
6793 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
6794 * resolved along body-frame axes, averaged over time interval and
6795 * expressed in meters per squared second (m/s^2).
6796 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
6797 * resolved along body-frame axes, averaged over time interval and
6798 * expressed in radians per second (rad/s).
6799 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
6800 * resolved along body-frame axes, averaged over time interval and
6801 * expressed in radians per second (rad/s).
6802 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
6803 * resolved along body-frame axes, averaged over time interval and
6804 * expressed in radians per second (rad/s).
6805 * @return estimated NED frame containing new body position, velocity and coordinate
6806 * transformation matrix.
6807 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
6808 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
6809 * body-to-NED-frame coordinate transformation matrix are
6810 * invalid.
6811 */
6812 public NEDFrame navigateAndReturnNew(
6813 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
6814 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
6815 final double fx, final double fy, final double fz,
6816 final double angularRateX, final double angularRateY, final double angularRateZ)
6817 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
6818 return navigateAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
6819 fx, fy, fz, angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
6820 }
6821
6822 /**
6823 * Runs precision local-navigation-frame inertial navigation equations.
6824 * NOTE: only the attitude update and specific force frame transformation
6825 * phases are precise.
6826 *
6827 * @param timeInterval time interval between epochs expressed in seconds (s).
6828 * @param oldPosition previous curvilinear position expressed in terms of latitude,
6829 * longitude and height.
6830 * @param oldC previous body-to-NED coordinate transformation.
6831 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
6832 * resolved along NED-frame axes and expressed in meters per second (m/s).
6833 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
6834 * resolved along NED-frame axes and expressed in meters per second (m/s).
6835 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
6836 * resolved along NED-frame axes and expressed in meters per second (m/s).
6837 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
6838 * resolved along body-frame axes, averaged over time interval and
6839 * expressed in meters per squared second (m/s^2).
6840 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
6841 * resolved along body-frame axes, averaged over time interval and
6842 * expressed in meters per squared second (m/s^2).
6843 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
6844 * resolved along body-frame axes, averaged over time interval and
6845 * expressed in meters per squared second (m/s^2).
6846 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
6847 * resolved along body-frame axes, averaged over time interval and
6848 * expressed in radians per second (rad/s).
6849 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
6850 * resolved along body-frame axes, averaged over time interval and
6851 * expressed in radians per second (rad/s).
6852 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
6853 * resolved along body-frame axes, averaged over time interval and
6854 * expressed in radians per second (rad/s).
6855 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
6856 * @return estimated NED frame containing new body position, velocity and coordinate
6857 * transformation matrix.
6858 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
6859 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
6860 * body-to-NED-frame coordinate transformation matrix are
6861 * invalid.
6862 */
6863 public NEDFrame navigateAndReturnNew(
6864 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
6865 final double oldVn, final double oldVe, final double oldVd,
6866 final double fx, final double fy, final double fz,
6867 final double angularRateX, final double angularRateY, final double angularRateZ,
6868 final double accuracyThreshold) throws InertialNavigatorException,
6869 InvalidSourceAndDestinationFrameTypeException {
6870 return navigateNEDAndReturnNew(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
6871 angularRateX, angularRateY, angularRateZ, accuracyThreshold);
6872 }
6873
6874 /**
6875 * Runs precision local-navigation-frame inertial navigation equations.
6876 * NOTE: only the attitude update and specific force frame transformation
6877 * phases are precise.
6878 *
6879 * @param timeInterval time interval between epochs expressed in seconds (s).
6880 * @param oldPosition previous curvilinear position expressed in terms of latitude,
6881 * longitude and height.
6882 * @param oldC previous body-to-NED coordinate transformation.
6883 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
6884 * resolved along NED-frame axes and expressed in meters per second (m/s).
6885 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
6886 * resolved along NED-frame axes and expressed in meters per second (m/s).
6887 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
6888 * resolved along NED-frame axes and expressed in meters per second (m/s).
6889 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
6890 * resolved along body-frame axes, averaged over time interval and
6891 * expressed in meters per squared second (m/s^2).
6892 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
6893 * resolved along body-frame axes, averaged over time interval and
6894 * expressed in meters per squared second (m/s^2).
6895 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
6896 * resolved along body-frame axes, averaged over time interval and
6897 * expressed in meters per squared second (m/s^2).
6898 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
6899 * resolved along body-frame axes, averaged over time interval and
6900 * expressed in radians per second (rad/s).
6901 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
6902 * resolved along body-frame axes, averaged over time interval and
6903 * expressed in radians per second (rad/s).
6904 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
6905 * resolved along body-frame axes, averaged over time interval and
6906 * expressed in radians per second (rad/s).
6907 * @return estimated NED frame containing new body position, velocity and coordinate
6908 * transformation matrix.
6909 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
6910 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
6911 * body-to-NED-frame coordinate transformation matrix are
6912 * invalid.
6913 */
6914 public NEDFrame navigateAndReturnNew(
6915 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
6916 final double oldVn, final double oldVe, final double oldVd,
6917 final double fx, final double fy, final double fz,
6918 final double angularRateX, final double angularRateY, final double angularRateZ)
6919 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
6920 return navigateAndReturnNew(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
6921 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
6922 }
6923
6924 /**
6925 * Runs precision local-navigation-frame inertial navigation equations.
6926 * NOTE: only the attitude update and specific force frame transformation
6927 * phases are precise.
6928 *
6929 * @param timeInterval time interval between epochs.
6930 * @param oldPosition previous curvilinear position expressed in terms of latitude,
6931 * longitude and height.
6932 * @param oldC previous body-to-NED coordinate transformation.
6933 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
6934 * resolved along NED-frame axes and expressed in meters per second (m/s).
6935 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
6936 * resolved along NED-frame axes and expressed in meters per second (m/s).
6937 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
6938 * resolved along NED-frame axes and expressed in meters per second (m/s).
6939 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
6940 * resolved along body-frame axes, averaged over time interval and
6941 * expressed in meters per squared second (m/s^2).
6942 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
6943 * resolved along body-frame axes, averaged over time interval and
6944 * expressed in meters per squared second (m/s^2).
6945 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
6946 * resolved along body-frame axes, averaged over time interval and
6947 * expressed in meters per squared second (m/s^2).
6948 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
6949 * resolved along body-frame axes, averaged over time interval and
6950 * expressed in radians per second (rad/s).
6951 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
6952 * resolved along body-frame axes, averaged over time interval and
6953 * expressed in radians per second (rad/s).
6954 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
6955 * resolved along body-frame axes, averaged over time interval and
6956 * expressed in radians per second (rad/s).
6957 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
6958 * @return estimated NED frame containing new body position, velocity and coordinate
6959 * transformation matrix.
6960 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
6961 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
6962 * body-to-NED-frame coordinate transformation matrix are
6963 * invalid.
6964 */
6965 public NEDFrame navigateAndReturnNew(
6966 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
6967 final double oldVn, final double oldVe, final double oldVd,
6968 final double fx, final double fy, final double fz,
6969 final double angularRateX, final double angularRateY, final double angularRateZ,
6970 final double accuracyThreshold) throws InertialNavigatorException,
6971 InvalidSourceAndDestinationFrameTypeException {
6972 return navigateNEDAndReturnNew(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
6973 angularRateX, angularRateY, angularRateZ, accuracyThreshold);
6974 }
6975
6976 /**
6977 * Runs precision local-navigation-frame inertial navigation equations.
6978 * NOTE: only the attitude update and specific force frame transformation
6979 * phases are precise.
6980 *
6981 * @param timeInterval time interval between epochs.
6982 * @param oldPosition previous curvilinear position expressed in terms of latitude,
6983 * longitude and height.
6984 * @param oldC previous body-to-NED coordinate transformation.
6985 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
6986 * resolved along NED-frame axes and expressed in meters per second (m/s).
6987 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
6988 * resolved along NED-frame axes and expressed in meters per second (m/s).
6989 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
6990 * resolved along NED-frame axes and expressed in meters per second (m/s).
6991 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
6992 * resolved along body-frame axes, averaged over time interval and
6993 * expressed in meters per squared second (m/s^2).
6994 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
6995 * resolved along body-frame axes, averaged over time interval and
6996 * expressed in meters per squared second (m/s^2).
6997 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
6998 * resolved along body-frame axes, averaged over time interval and
6999 * expressed in meters per squared second (m/s^2).
7000 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
7001 * resolved along body-frame axes, averaged over time interval and
7002 * expressed in radians per second (rad/s).
7003 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
7004 * resolved along body-frame axes, averaged over time interval and
7005 * expressed in radians per second (rad/s).
7006 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
7007 * resolved along body-frame axes, averaged over time interval and
7008 * expressed in radians per second (rad/s).
7009 * @return estimated NED frame containing new body position, velocity and coordinate
7010 * transformation matrix.
7011 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
7012 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
7013 * body-to-NED-frame coordinate transformation matrix are
7014 * invalid.
7015 */
7016 public NEDFrame navigateAndReturnNew(
7017 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
7018 final double oldVn, final double oldVe, final double oldVd,
7019 final double fx, final double fy, final double fz,
7020 final double angularRateX, final double angularRateY, final double angularRateZ)
7021 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
7022 return navigateAndReturnNew(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
7023 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
7024 }
7025
7026 /**
7027 * Runs precision local-navigation-frame inertial navigation equations.
7028 * NOTE: only the attitude update and specific force frame transformation
7029 * phases are precise.
7030 *
7031 * @param timeInterval time interval between epochs expressed in seconds (s).
7032 * @param oldLatitude previous latitude expressed in radians (rad).
7033 * @param oldLongitude previous longitude expressed in radians (rad).
7034 * @param oldHeight previous height expressed in meters (m).
7035 * @param oldC previous body-to-NED coordinate transformation.
7036 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
7037 * along north, east and down axes.
7038 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
7039 * resolved along body-frame axes, averaged over time interval and
7040 * expressed in meters per squared second (m/s^2).
7041 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
7042 * resolved along body-frame axes, averaged over time interval and
7043 * expressed in meters per squared second (m/s^2).
7044 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
7045 * resolved along body-frame axes, averaged over time interval and
7046 * expressed in meters per squared second (m/s^2).
7047 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
7048 * resolved along body-frame axes, averaged over time interval and
7049 * expressed in radians per second (rad/s).
7050 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
7051 * resolved along body-frame axes, averaged over time interval and
7052 * expressed in radians per second (rad/s).
7053 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
7054 * resolved along body-frame axes, averaged over time interval and
7055 * expressed in radians per second (rad/s).
7056 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
7057 * @return estimated NED frame containing new body position, velocity and coordinate
7058 * transformation matrix.
7059 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
7060 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
7061 * body-to-NED-frame coordinate transformation matrix are
7062 * invalid.
7063 */
7064 public NEDFrame navigateAndReturnNew(
7065 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
7066 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
7067 final double fx, final double fy, final double fz,
7068 final double angularRateX, final double angularRateY, final double angularRateZ,
7069 final double accuracyThreshold) throws InertialNavigatorException,
7070 InvalidSourceAndDestinationFrameTypeException {
7071 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity,
7072 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold);
7073 }
7074
7075 /**
7076 * Runs precision local-navigation-frame inertial navigation equations.
7077 * NOTE: only the attitude update and specific force frame transformation
7078 * phases are precise.
7079 *
7080 * @param timeInterval time interval between epochs expressed in seconds (s).
7081 * @param oldLatitude previous latitude expressed in radians (rad).
7082 * @param oldLongitude previous longitude expressed in radians (rad).
7083 * @param oldHeight previous height expressed in meters (m).
7084 * @param oldC previous body-to-NED coordinate transformation.
7085 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
7086 * along north, east and down axes.
7087 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
7088 * resolved along body-frame axes, averaged over time interval and
7089 * expressed in meters per squared second (m/s^2).
7090 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
7091 * resolved along body-frame axes, averaged over time interval and
7092 * expressed in meters per squared second (m/s^2).
7093 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
7094 * resolved along body-frame axes, averaged over time interval and
7095 * expressed in meters per squared second (m/s^2).
7096 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
7097 * resolved along body-frame axes, averaged over time interval and
7098 * expressed in radians per second (rad/s).
7099 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
7100 * resolved along body-frame axes, averaged over time interval and
7101 * expressed in radians per second (rad/s).
7102 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
7103 * resolved along body-frame axes, averaged over time interval and
7104 * expressed in radians per second (rad/s).
7105 * @return estimated NED frame containing new body position, velocity and coordinate
7106 * transformation matrix.
7107 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
7108 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
7109 * body-to-NED-frame coordinate transformation matrix are
7110 * invalid.
7111 */
7112 public NEDFrame navigateAndReturnNew(
7113 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
7114 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
7115 final double fx, final double fy, final double fz,
7116 final double angularRateX, final double angularRateY, final double angularRateZ)
7117 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
7118 return navigateAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity,
7119 fx, fy, fz, angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
7120 }
7121
7122 /**
7123 * Runs precision local-navigation-frame inertial navigation equations.
7124 * NOTE: only the attitude update and specific force frame transformation
7125 * phases are precise.
7126 *
7127 * @param timeInterval time interval between epochs.
7128 * @param oldLatitude previous latitude expressed in radians (rad).
7129 * @param oldLongitude previous longitude expressed in radians (rad).
7130 * @param oldHeight previous height expressed in meters (m).
7131 * @param oldC previous body-to-NED coordinate transformation.
7132 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
7133 * along north, east and down axes.
7134 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
7135 * resolved along body-frame axes, averaged over time interval and
7136 * expressed in meters per squared second (m/s^2).
7137 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
7138 * resolved along body-frame axes, averaged over time interval and
7139 * expressed in meters per squared second (m/s^2).
7140 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
7141 * resolved along body-frame axes, averaged over time interval and
7142 * expressed in meters per squared second (m/s^2).
7143 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
7144 * resolved along body-frame axes, averaged over time interval and
7145 * expressed in radians per second (rad/s).
7146 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
7147 * resolved along body-frame axes, averaged over time interval and
7148 * expressed in radians per second (rad/s).
7149 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
7150 * resolved along body-frame axes, averaged over time interval and
7151 * expressed in radians per second (rad/s).
7152 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
7153 * @return estimated NED frame containing new body position, velocity and coordinate
7154 * transformation matrix.
7155 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
7156 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
7157 * body-to-NED-frame coordinate transformation matrix are
7158 * invalid.
7159 */
7160 public NEDFrame navigateAndReturnNew(
7161 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
7162 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
7163 final double fx, final double fy, final double fz,
7164 final double angularRateX, final double angularRateY, final double angularRateZ,
7165 final double accuracyThreshold) throws InertialNavigatorException,
7166 InvalidSourceAndDestinationFrameTypeException {
7167 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity,
7168 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold);
7169 }
7170
7171 /**
7172 * Runs precision local-navigation-frame inertial navigation equations.
7173 * NOTE: only the attitude update and specific force frame transformation
7174 * phases are precise.
7175 *
7176 * @param timeInterval time interval between epochs.
7177 * @param oldLatitude previous latitude expressed in radians (rad).
7178 * @param oldLongitude previous longitude expressed in radians (rad).
7179 * @param oldHeight previous height expressed in meters (m).
7180 * @param oldC previous body-to-NED coordinate transformation.
7181 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
7182 * along north, east and down axes.
7183 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
7184 * resolved along body-frame axes, averaged over time interval and
7185 * expressed in meters per squared second (m/s^2).
7186 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
7187 * resolved along body-frame axes, averaged over time interval and
7188 * expressed in meters per squared second (m/s^2).
7189 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
7190 * resolved along body-frame axes, averaged over time interval and
7191 * expressed in meters per squared second (m/s^2).
7192 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
7193 * resolved along body-frame axes, averaged over time interval and
7194 * expressed in radians per second (rad/s).
7195 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
7196 * resolved along body-frame axes, averaged over time interval and
7197 * expressed in radians per second (rad/s).
7198 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
7199 * resolved along body-frame axes, averaged over time interval and
7200 * expressed in radians per second (rad/s).
7201 * @return estimated NED frame containing new body position, velocity and coordinate
7202 * transformation matrix.
7203 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
7204 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
7205 * body-to-NED-frame coordinate transformation matrix are
7206 * invalid.
7207 */
7208 public NEDFrame navigateAndReturnNew(
7209 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
7210 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
7211 final double fx, final double fy, final double fz,
7212 final double angularRateX, final double angularRateY, final double angularRateZ)
7213 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
7214 return navigateAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity,
7215 fx, fy, fz, angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
7216 }
7217
7218 /**
7219 * Runs precision local-navigation-frame inertial navigation equations.
7220 * NOTE: only the attitude update and specific force frame transformation
7221 * phases are precise.
7222 *
7223 * @param timeInterval time interval between epochs expressed in seconds (s).
7224 * @param oldPosition previous curvilinear position expressed in terms of latitude,
7225 * longitude and height.
7226 * @param oldC previous body-to-NED coordinate transformation.
7227 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
7228 * along north, east and down axes.
7229 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
7230 * resolved along body-frame axes, averaged over time interval and
7231 * expressed in meters per squared second (m/s^2).
7232 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
7233 * resolved along body-frame axes, averaged over time interval and
7234 * expressed in meters per squared second (m/s^2).
7235 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
7236 * resolved along body-frame axes, averaged over time interval and
7237 * expressed in meters per squared second (m/s^2).
7238 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
7239 * resolved along body-frame axes, averaged over time interval and
7240 * expressed in radians per second (rad/s).
7241 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
7242 * resolved along body-frame axes, averaged over time interval and
7243 * expressed in radians per second (rad/s).
7244 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
7245 * resolved along body-frame axes, averaged over time interval and
7246 * expressed in radians per second (rad/s).
7247 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
7248 * @return estimated NED frame containing new body position, velocity and coordinate
7249 * transformation matrix.
7250 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
7251 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
7252 * body-to-NED-frame coordinate transformation matrix are
7253 * invalid.
7254 */
7255 public NEDFrame navigateAndReturnNew(
7256 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
7257 final NEDVelocity oldVelocity, final double fx, final double fy, final double fz,
7258 final double angularRateX, final double angularRateY, final double angularRateZ,
7259 final double accuracyThreshold) throws InertialNavigatorException,
7260 InvalidSourceAndDestinationFrameTypeException {
7261 return navigateNEDAndReturnNew(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz,
7262 angularRateX, angularRateY, angularRateZ, accuracyThreshold);
7263 }
7264
7265 /**
7266 * Runs precision local-navigation-frame inertial navigation equations.
7267 * NOTE: only the attitude update and specific force frame transformation
7268 * phases are precise.
7269 *
7270 * @param timeInterval time interval between epochs expressed in seconds (s).
7271 * @param oldPosition previous curvilinear position expressed in terms of latitude,
7272 * longitude and height.
7273 * @param oldC previous body-to-NED coordinate transformation.
7274 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
7275 * along north, east and down axes.
7276 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
7277 * resolved along body-frame axes, averaged over time interval and
7278 * expressed in meters per squared second (m/s^2).
7279 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
7280 * resolved along body-frame axes, averaged over time interval and
7281 * expressed in meters per squared second (m/s^2).
7282 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
7283 * resolved along body-frame axes, averaged over time interval and
7284 * expressed in meters per squared second (m/s^2).
7285 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
7286 * resolved along body-frame axes, averaged over time interval and
7287 * expressed in radians per second (rad/s).
7288 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
7289 * resolved along body-frame axes, averaged over time interval and
7290 * expressed in radians per second (rad/s).
7291 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
7292 * resolved along body-frame axes, averaged over time interval and
7293 * expressed in radians per second (rad/s).
7294 * @return estimated NED frame containing new body position, velocity and coordinate
7295 * transformation matrix.
7296 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
7297 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
7298 * body-to-NED-frame coordinate transformation matrix are
7299 * invalid.
7300 */
7301 public NEDFrame navigateAndReturnNew(
7302 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
7303 final NEDVelocity oldVelocity, final double fx, final double fy, final double fz,
7304 final double angularRateX, final double angularRateY, final double angularRateZ)
7305 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
7306 return navigateAndReturnNew(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz,
7307 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
7308 }
7309
7310 /**
7311 * Runs precision local-navigation-frame inertial navigation equations.
7312 * NOTE: only the attitude update and specific force frame transformation
7313 * phases are precise.
7314 *
7315 * @param timeInterval time interval between epochs.
7316 * @param oldPosition previous curvilinear position expressed in terms of latitude,
7317 * longitude and height.
7318 * @param oldC previous body-to-NED coordinate transformation.
7319 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
7320 * along north, east and down axes.
7321 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
7322 * resolved along body-frame axes, averaged over time interval and
7323 * expressed in meters per squared second (m/s^2).
7324 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
7325 * resolved along body-frame axes, averaged over time interval and
7326 * expressed in meters per squared second (m/s^2).
7327 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
7328 * resolved along body-frame axes, averaged over time interval and
7329 * expressed in meters per squared second (m/s^2).
7330 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
7331 * resolved along body-frame axes, averaged over time interval and
7332 * expressed in radians per second (rad/s).
7333 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
7334 * resolved along body-frame axes, averaged over time interval and
7335 * expressed in radians per second (rad/s).
7336 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
7337 * resolved along body-frame axes, averaged over time interval and
7338 * expressed in radians per second (rad/s).
7339 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
7340 * @return estimated NED frame containing new body position, velocity and coordinate
7341 * transformation matrix.
7342 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
7343 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
7344 * body-to-NED-frame coordinate transformation matrix are
7345 * invalid.
7346 */
7347 public NEDFrame navigateAndReturnNew(
7348 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
7349 final NEDVelocity oldVelocity, final double fx, final double fy, final double fz,
7350 final double angularRateX, final double angularRateY, final double angularRateZ,
7351 final double accuracyThreshold) throws InertialNavigatorException,
7352 InvalidSourceAndDestinationFrameTypeException {
7353 return navigateNEDAndReturnNew(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz,
7354 angularRateX, angularRateY, angularRateZ, accuracyThreshold);
7355 }
7356
7357 /**
7358 * Runs precision local-navigation-frame inertial navigation equations.
7359 * NOTE: only the attitude update and specific force frame transformation
7360 * phases are precise.
7361 *
7362 * @param timeInterval time interval between epochs.
7363 * @param oldPosition previous curvilinear position expressed in terms of latitude,
7364 * longitude and height.
7365 * @param oldC previous body-to-NED coordinate transformation.
7366 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
7367 * along north, east and down axes.
7368 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
7369 * resolved along body-frame axes, averaged over time interval and
7370 * expressed in meters per squared second (m/s^2).
7371 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
7372 * resolved along body-frame axes, averaged over time interval and
7373 * expressed in meters per squared second (m/s^2).
7374 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
7375 * resolved along body-frame axes, averaged over time interval and
7376 * expressed in meters per squared second (m/s^2).
7377 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
7378 * resolved along body-frame axes, averaged over time interval and
7379 * expressed in radians per second (rad/s).
7380 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
7381 * resolved along body-frame axes, averaged over time interval and
7382 * expressed in radians per second (rad/s).
7383 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
7384 * resolved along body-frame axes, averaged over time interval and
7385 * expressed in radians per second (rad/s).
7386 * @return estimated NED frame containing new body position, velocity and coordinate
7387 * transformation matrix.
7388 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
7389 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
7390 * body-to-NED-frame coordinate transformation matrix are
7391 * invalid.
7392 */
7393 public NEDFrame navigateAndReturnNew(
7394 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
7395 final NEDVelocity oldVelocity, final double fx, final double fy, final double fz,
7396 final double angularRateX, final double angularRateY, final double angularRateZ)
7397 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
7398 return navigateAndReturnNew(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz,
7399 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
7400 }
7401
7402 /**
7403 * Runs precision local-navigation-frame inertial navigation equations.
7404 * NOTE: only the attitude update and specific force frame transformation
7405 * phases are precise.
7406 *
7407 * @param timeInterval time interval between epochs expressed in seconds (s).
7408 * @param oldLatitude previous latitude expressed in radians (rad).
7409 * @param oldLongitude previous longitude expressed in radians (rad).
7410 * @param oldHeight previous height expressed in meters (m).
7411 * @param oldC previous body-to-NED coordinate transformation.
7412 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
7413 * resolved along NED-frame axes and expressed in meters per second (m/s).
7414 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
7415 * resolved along NED-frame axes and expressed in meters per second (m/s).
7416 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
7417 * resolved along NED-frame axes and expressed in meters per second (m/s).
7418 * @param kinematics body kinematics containing specific forces and angular rates applied to
7419 * the body.
7420 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
7421 * @return estimated NED frame containing new body position, velocity and coordinate
7422 * transformation matrix.
7423 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
7424 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
7425 * body-to-NED-frame coordinate transformation matrix are
7426 * invalid.
7427 */
7428 public NEDFrame navigateAndReturnNew(
7429 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
7430 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
7431 final BodyKinematics kinematics, final double accuracyThreshold) throws InertialNavigatorException,
7432 InvalidSourceAndDestinationFrameTypeException {
7433 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
7434 kinematics, accuracyThreshold);
7435 }
7436
7437 /**
7438 * Runs precision local-navigation-frame inertial navigation equations.
7439 * NOTE: only the attitude update and specific force frame transformation
7440 * phases are precise.
7441 *
7442 * @param timeInterval time interval between epochs expressed in seconds (s).
7443 * @param oldLatitude previous latitude expressed in radians (rad).
7444 * @param oldLongitude previous longitude expressed in radians (rad).
7445 * @param oldHeight previous height expressed in meters (m).
7446 * @param oldC previous body-to-NED coordinate transformation.
7447 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
7448 * resolved along NED-frame axes and expressed in meters per second (m/s).
7449 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
7450 * resolved along NED-frame axes and expressed in meters per second (m/s).
7451 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
7452 * resolved along NED-frame axes and expressed in meters per second (m/s).
7453 * @param kinematics body kinematics containing specific forces and angular rates applied to
7454 * the body.
7455 * @return estimated NED frame containing new body position, velocity and coordinate
7456 * transformation matrix.
7457 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
7458 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
7459 * body-to-NED-frame coordinate transformation matrix are
7460 * invalid.
7461 */
7462 public NEDFrame navigateAndReturnNew(
7463 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
7464 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
7465 final BodyKinematics kinematics) throws InertialNavigatorException,
7466 InvalidSourceAndDestinationFrameTypeException {
7467 return navigateAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
7468 kinematics, DEFAULT_ACCURACY_THRESHOLD);
7469 }
7470
7471 /**
7472 * Runs precision local-navigation-frame inertial navigation equations.
7473 * NOTE: only the attitude update and specific force frame transformation
7474 * phases are precise.
7475 *
7476 * @param timeInterval time interval between epochs.
7477 * @param oldLatitude previous latitude expressed in radians (rad).
7478 * @param oldLongitude previous longitude expressed in radians (rad).
7479 * @param oldHeight previous height expressed in meters (m).
7480 * @param oldC previous body-to-NED coordinate transformation.
7481 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
7482 * resolved along NED-frame axes and expressed in meters per second (m/s).
7483 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
7484 * resolved along NED-frame axes and expressed in meters per second (m/s).
7485 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
7486 * resolved along NED-frame axes and expressed in meters per second (m/s).
7487 * @param kinematics body kinematics containing specific forces and angular rates applied to
7488 * the body.
7489 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
7490 * @return estimated NED frame containing new body position, velocity and coordinate
7491 * transformation matrix.
7492 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
7493 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
7494 * body-to-NED-frame coordinate transformation matrix are
7495 * invalid.
7496 */
7497 public NEDFrame navigateAndReturnNew(
7498 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
7499 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
7500 final BodyKinematics kinematics, final double accuracyThreshold) throws InertialNavigatorException,
7501 InvalidSourceAndDestinationFrameTypeException {
7502 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
7503 kinematics, accuracyThreshold);
7504 }
7505
7506 /**
7507 * Runs precision local-navigation-frame inertial navigation equations.
7508 * NOTE: only the attitude update and specific force frame transformation
7509 * phases are precise.
7510 *
7511 * @param timeInterval time interval between epochs.
7512 * @param oldLatitude previous latitude expressed in radians (rad).
7513 * @param oldLongitude previous longitude expressed in radians (rad).
7514 * @param oldHeight previous height expressed in meters (m).
7515 * @param oldC previous body-to-NED coordinate transformation.
7516 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
7517 * resolved along NED-frame axes and expressed in meters per second (m/s).
7518 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
7519 * resolved along NED-frame axes and expressed in meters per second (m/s).
7520 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
7521 * resolved along NED-frame axes and expressed in meters per second (m/s).
7522 * @param kinematics body kinematics containing specific forces and angular rates applied to
7523 * the body.
7524 * @return estimated NED frame containing new body position, velocity and coordinate
7525 * transformation matrix.
7526 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
7527 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
7528 * body-to-NED-frame coordinate transformation matrix are
7529 * invalid.
7530 */
7531 public NEDFrame navigateAndReturnNew(
7532 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
7533 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
7534 final BodyKinematics kinematics) throws InertialNavigatorException,
7535 InvalidSourceAndDestinationFrameTypeException {
7536 return navigateAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
7537 kinematics, DEFAULT_ACCURACY_THRESHOLD);
7538 }
7539
7540 /**
7541 * Runs precision local-navigation-frame inertial navigation equations.
7542 * NOTE: only the attitude update and specific force frame transformation
7543 * phases are precise.
7544 *
7545 * @param timeInterval time interval between epochs expressed in seconds (s).
7546 * @param oldPosition previous curvilinear position expressed in terms of latitude,
7547 * longitude and height.
7548 * @param oldC previous body-to-NED coordinate transformation.
7549 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
7550 * resolved along NED-frame axes and expressed in meters per second (m/s).
7551 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
7552 * resolved along NED-frame axes and expressed in meters per second (m/s).
7553 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
7554 * resolved along NED-frame axes and expressed in meters per second (m/s).
7555 * @param kinematics body kinematics containing specific forces and angular rates applied to
7556 * the body.
7557 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
7558 * @return estimated NED frame containing new body position, velocity and coordinate
7559 * transformation matrix.
7560 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
7561 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
7562 * body-to-NED-frame coordinate transformation matrix are
7563 * invalid.
7564 */
7565 public NEDFrame navigateAndReturnNew(
7566 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
7567 final double oldVn, final double oldVe, final double oldVd, final BodyKinematics kinematics,
7568 final double accuracyThreshold) throws InertialNavigatorException,
7569 InvalidSourceAndDestinationFrameTypeException {
7570 return navigateNEDAndReturnNew(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, kinematics,
7571 accuracyThreshold);
7572 }
7573
7574 /**
7575 * Runs precision local-navigation-frame inertial navigation equations.
7576 * NOTE: only the attitude update and specific force frame transformation
7577 * phases are precise.
7578 *
7579 * @param timeInterval time interval between epochs expressed in seconds (s).
7580 * @param oldPosition previous curvilinear position expressed in terms of latitude,
7581 * longitude and height.
7582 * @param oldC previous body-to-NED coordinate transformation.
7583 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
7584 * resolved along NED-frame axes and expressed in meters per second (m/s).
7585 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
7586 * resolved along NED-frame axes and expressed in meters per second (m/s).
7587 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
7588 * resolved along NED-frame axes and expressed in meters per second (m/s).
7589 * @param kinematics body kinematics containing specific forces and angular rates applied to
7590 * the body.
7591 * @return estimated NED frame containing new body position, velocity and coordinate
7592 * transformation matrix.
7593 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
7594 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
7595 * body-to-NED-frame coordinate transformation matrix are
7596 * invalid.
7597 */
7598 public NEDFrame navigateAndReturnNew(
7599 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
7600 final double oldVn, final double oldVe, final double oldVd, final BodyKinematics kinematics)
7601 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
7602 return navigateAndReturnNew(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, kinematics,
7603 DEFAULT_ACCURACY_THRESHOLD);
7604 }
7605
7606 /**
7607 * Runs precision local-navigation-frame inertial navigation equations.
7608 * NOTE: only the attitude update and specific force frame transformation
7609 * phases are precise.
7610 *
7611 * @param timeInterval time interval between epochs.
7612 * @param oldPosition previous curvilinear position expressed in terms of latitude,
7613 * longitude and height.
7614 * @param oldC previous body-to-NED coordinate transformation.
7615 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
7616 * resolved along NED-frame axes and expressed in meters per second (m/s).
7617 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
7618 * resolved along NED-frame axes and expressed in meters per second (m/s).
7619 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
7620 * resolved along NED-frame axes and expressed in meters per second (m/s).
7621 * @param kinematics body kinematics containing specific forces and angular rates applied to
7622 * the body.
7623 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
7624 * @return estimated NED frame containing new body position, velocity and coordinate
7625 * transformation matrix.
7626 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
7627 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
7628 * body-to-NED-frame coordinate transformation matrix are
7629 * invalid.
7630 */
7631 public NEDFrame navigateAndReturnNew(
7632 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
7633 final double oldVn, final double oldVe, final double oldVd, final BodyKinematics kinematics,
7634 final double accuracyThreshold) throws InertialNavigatorException,
7635 InvalidSourceAndDestinationFrameTypeException {
7636 return navigateNEDAndReturnNew(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, kinematics,
7637 accuracyThreshold);
7638 }
7639
7640 /**
7641 * Runs precision local-navigation-frame inertial navigation equations.
7642 * NOTE: only the attitude update and specific force frame transformation
7643 * phases are precise.
7644 *
7645 * @param timeInterval time interval between epochs.
7646 * @param oldPosition previous curvilinear position expressed in terms of latitude,
7647 * longitude and height.
7648 * @param oldC previous body-to-NED coordinate transformation.
7649 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
7650 * resolved along NED-frame axes and expressed in meters per second (m/s).
7651 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
7652 * resolved along NED-frame axes and expressed in meters per second (m/s).
7653 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
7654 * resolved along NED-frame axes and expressed in meters per second (m/s).
7655 * @param kinematics body kinematics containing specific forces and angular rates applied to
7656 * the body.
7657 * @return estimated NED frame containing new body position, velocity and coordinate
7658 * transformation matrix.
7659 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
7660 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
7661 * body-to-NED-frame coordinate transformation matrix are
7662 * invalid.
7663 */
7664 public NEDFrame navigateAndReturnNew(
7665 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
7666 final double oldVn, final double oldVe, final double oldVd, final BodyKinematics kinematics)
7667 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
7668 return navigateAndReturnNew(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, kinematics,
7669 DEFAULT_ACCURACY_THRESHOLD);
7670 }
7671
7672 /**
7673 * Runs precision local-navigation-frame inertial navigation equations.
7674 * NOTE: only the attitude update and specific force frame transformation
7675 * phases are precise.
7676 *
7677 * @param timeInterval time interval between epochs expressed in seconds (s).
7678 * @param oldLatitude previous latitude expressed in radians (rad).
7679 * @param oldLongitude previous longitude expressed in radians (rad).
7680 * @param oldHeight previous height expressed in meters (m).
7681 * @param oldC previous body-to-NED coordinate transformation.
7682 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
7683 * along north, east and down axes.
7684 * @param kinematics body kinematics containing specific forces and angular rates applied to
7685 * the body.
7686 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
7687 * @return estimated NED frame containing new body position, velocity and coordinate
7688 * transformation matrix.
7689 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
7690 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
7691 * body-to-NED-frame coordinate transformation matrix are
7692 * invalid.
7693 */
7694 public NEDFrame navigateAndReturnNew(
7695 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
7696 final CoordinateTransformation oldC, final NEDVelocity oldVelocity, final BodyKinematics kinematics,
7697 final double accuracyThreshold) throws InertialNavigatorException,
7698 InvalidSourceAndDestinationFrameTypeException {
7699 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity,
7700 kinematics, accuracyThreshold);
7701 }
7702
7703 /**
7704 * Runs precision local-navigation-frame inertial navigation equations.
7705 * NOTE: only the attitude update and specific force frame transformation
7706 * phases are precise.
7707 *
7708 * @param timeInterval time interval between epochs expressed in seconds (s).
7709 * @param oldLatitude previous latitude expressed in radians (rad).
7710 * @param oldLongitude previous longitude expressed in radians (rad).
7711 * @param oldHeight previous height expressed in meters (m).
7712 * @param oldC previous body-to-NED coordinate transformation.
7713 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
7714 * along north, east and down axes.
7715 * @param kinematics body kinematics containing specific forces and angular rates applied to
7716 * the body.
7717 * @return estimated NED frame containing new body position, velocity and coordinate
7718 * transformation matrix.
7719 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
7720 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
7721 * body-to-NED-frame coordinate transformation matrix are
7722 * invalid.
7723 */
7724 public NEDFrame navigateAndReturnNew(
7725 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
7726 final CoordinateTransformation oldC, final NEDVelocity oldVelocity, final BodyKinematics kinematics)
7727 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
7728 return navigateAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, kinematics,
7729 DEFAULT_ACCURACY_THRESHOLD);
7730 }
7731
7732 /**
7733 * Runs precision local-navigation-frame inertial navigation equations.
7734 * NOTE: only the attitude update and specific force frame transformation
7735 * phases are precise.
7736 *
7737 * @param timeInterval time interval between epochs.
7738 * @param oldLatitude previous latitude expressed in radians (rad).
7739 * @param oldLongitude previous longitude expressed in radians (rad).
7740 * @param oldHeight previous height expressed in meters (m).
7741 * @param oldC previous body-to-NED coordinate transformation.
7742 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
7743 * along north, east and down axes.
7744 * @param kinematics body kinematics containing specific forces and angular rates applied to
7745 * the body.
7746 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
7747 * @return estimated NED frame containing new body position, velocity and coordinate
7748 * transformation matrix.
7749 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
7750 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
7751 * body-to-NED-frame coordinate transformation matrix are
7752 * invalid.
7753 */
7754 public NEDFrame navigateAndReturnNew(
7755 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
7756 final CoordinateTransformation oldC, final NEDVelocity oldVelocity, final BodyKinematics kinematics,
7757 final double accuracyThreshold) throws InertialNavigatorException,
7758 InvalidSourceAndDestinationFrameTypeException {
7759 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity,
7760 kinematics, accuracyThreshold);
7761 }
7762
7763 /**
7764 * Runs precision local-navigation-frame inertial navigation equations.
7765 * NOTE: only the attitude update and specific force frame transformation
7766 * phases are precise.
7767 *
7768 * @param timeInterval time interval between epochs.
7769 * @param oldLatitude previous latitude expressed in radians (rad).
7770 * @param oldLongitude previous longitude expressed in radians (rad).
7771 * @param oldHeight previous height expressed in meters (m).
7772 * @param oldC previous body-to-NED coordinate transformation.
7773 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
7774 * along north, east and down axes.
7775 * @param kinematics body kinematics containing specific forces and angular rates applied to
7776 * the body.
7777 * @return estimated NED frame containing new body position, velocity and coordinate
7778 * transformation matrix.
7779 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
7780 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
7781 * body-to-NED-frame coordinate transformation matrix are
7782 * invalid.
7783 */
7784 public NEDFrame navigateAndReturnNew(
7785 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
7786 final CoordinateTransformation oldC, final NEDVelocity oldVelocity, final BodyKinematics kinematics)
7787 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
7788 return navigateAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, kinematics,
7789 DEFAULT_ACCURACY_THRESHOLD);
7790 }
7791
7792 /**
7793 * Runs precision local-navigation-frame inertial navigation equations.
7794 * NOTE: only the attitude update and specific force frame transformation
7795 * phases are precise.
7796 *
7797 * @param timeInterval time interval between epochs expressed in seconds (s).
7798 * @param oldPosition previous curvilinear position expressed in terms of latitude,
7799 * longitude and height.
7800 * @param oldC previous body-to-NED coordinate transformation.
7801 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
7802 * along north, east and down axes.
7803 * @param kinematics body kinematics containing specific forces and angular rates applied to
7804 * the body.
7805 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
7806 * @return estimated NED frame containing new body position, velocity and coordinate
7807 * transformation matrix.
7808 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
7809 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
7810 * body-to-NED-frame coordinate transformation matrix are
7811 * invalid.
7812 */
7813 public NEDFrame navigateAndReturnNew(
7814 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
7815 final NEDVelocity oldVelocity, final BodyKinematics kinematics, final double accuracyThreshold)
7816 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
7817 return navigateNEDAndReturnNew(timeInterval, oldPosition, oldC, oldVelocity, kinematics, accuracyThreshold);
7818 }
7819
7820 /**
7821 * Runs precision local-navigation-frame inertial navigation equations.
7822 * NOTE: only the attitude update and specific force frame transformation
7823 * phases are precise.
7824 *
7825 * @param timeInterval time interval between epochs expressed in seconds (s).
7826 * @param oldPosition previous curvilinear position expressed in terms of latitude,
7827 * longitude and height.
7828 * @param oldC previous body-to-NED coordinate transformation.
7829 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
7830 * along north, east and down axes.
7831 * @param kinematics body kinematics containing specific forces and angular rates applied to
7832 * the body.
7833 * @return estimated NED frame containing new body position, velocity and coordinate
7834 * transformation matrix.
7835 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
7836 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
7837 * body-to-NED-frame coordinate transformation matrix are
7838 * invalid.
7839 */
7840 public NEDFrame navigateAndReturnNew(
7841 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
7842 final NEDVelocity oldVelocity, final BodyKinematics kinematics) throws InertialNavigatorException,
7843 InvalidSourceAndDestinationFrameTypeException {
7844 return navigateAndReturnNew(timeInterval, oldPosition, oldC, oldVelocity, kinematics,
7845 DEFAULT_ACCURACY_THRESHOLD);
7846 }
7847
7848 /**
7849 * Runs precision local-navigation-frame inertial navigation equations.
7850 * NOTE: only the attitude update and specific force frame transformation
7851 * phases are precise.
7852 *
7853 * @param timeInterval time interval between epochs expressed in seconds (s).
7854 * @param oldPosition previous curvilinear position expressed in terms of latitude,
7855 * longitude and height.
7856 * @param oldC previous body-to-NED coordinate transformation.
7857 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
7858 * along north, east and down axes.
7859 * @param kinematics body kinematics containing specific forces and angular rates applied to
7860 * the body.
7861 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
7862 * @return estimated NED frame containing new body position, velocity and coordinate
7863 * transformation matrix.
7864 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
7865 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
7866 * body-to-NED-frame coordinate transformation matrix are
7867 * invalid.
7868 */
7869 public NEDFrame navigateAndReturnNew(
7870 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
7871 final NEDVelocity oldVelocity, final BodyKinematics kinematics, final double accuracyThreshold)
7872 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
7873 return navigateNEDAndReturnNew(timeInterval, oldPosition, oldC, oldVelocity, kinematics, accuracyThreshold);
7874 }
7875
7876 /**
7877 * Runs precision local-navigation-frame inertial navigation equations.
7878 * NOTE: only the attitude update and specific force frame transformation
7879 * phases are precise.
7880 *
7881 * @param timeInterval time interval between epochs expressed in seconds (s).
7882 * @param oldPosition previous curvilinear position expressed in terms of latitude,
7883 * longitude and height.
7884 * @param oldC previous body-to-NED coordinate transformation.
7885 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
7886 * along north, east and down axes.
7887 * @param kinematics body kinematics containing specific forces and angular rates applied to
7888 * the body.
7889 * @return estimated NED frame containing new body position, velocity and coordinate
7890 * transformation matrix.
7891 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
7892 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
7893 * body-to-NED-frame coordinate transformation matrix are
7894 * invalid.
7895 */
7896 public NEDFrame navigateAndReturnNew(
7897 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
7898 final NEDVelocity oldVelocity, final BodyKinematics kinematics) throws InertialNavigatorException,
7899 InvalidSourceAndDestinationFrameTypeException {
7900 return navigateAndReturnNew(timeInterval, oldPosition, oldC, oldVelocity, kinematics,
7901 DEFAULT_ACCURACY_THRESHOLD);
7902 }
7903
7904 /**
7905 * Runs precision local-navigation-frame inertial navigation equations.
7906 * NOTE: only the attitude update and specific force frame transformation
7907 * phases are precise.
7908 *
7909 * @param timeInterval time interval between epochs expressed in seconds (s).
7910 * @param oldLatitude previous latitude angle.
7911 * @param oldLongitude previous longitude angle.
7912 * @param oldHeight previous height.
7913 * @param oldC previous body-to-NED coordinate transformation.
7914 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
7915 * resolved along NED-frame axes and expressed in meters per second (m/s).
7916 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
7917 * resolved along NED-frame axes and expressed in meters per second (m/s).
7918 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
7919 * resolved along NED-frame axes and expressed in meters per second (m/s).
7920 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
7921 * resolved along body-frame axes, averaged over time interval and
7922 * expressed in meters per squared second (m/s^2).
7923 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
7924 * resolved along body-frame axes, averaged over time interval and
7925 * expressed in meters per squared second (m/s^2).
7926 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
7927 * resolved along body-frame axes, averaged over time interval and
7928 * expressed in meters per squared second (m/s^2).
7929 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
7930 * resolved along body-frame axes, averaged over time interval and
7931 * expressed in radians per second (rad/s).
7932 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
7933 * resolved along body-frame axes, averaged over time interval and
7934 * expressed in radians per second (rad/s).
7935 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
7936 * resolved along body-frame axes, averaged over time interval and
7937 * expressed in radians per second (rad/s).
7938 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
7939 * @return estimated NED frame containing new body position, velocity and coordinate
7940 * transformation matrix.
7941 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
7942 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
7943 * body-to-NED-frame coordinate transformation matrix are
7944 * invalid.
7945 */
7946 public NEDFrame navigateAndReturnNew(
7947 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
7948 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
7949 final double fx, final double fy, final double fz,
7950 final double angularRateX, final double angularRateY, final double angularRateZ,
7951 final double accuracyThreshold) throws InertialNavigatorException,
7952 InvalidSourceAndDestinationFrameTypeException {
7953 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
7954 oldVn, oldVe, oldVd, fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold);
7955 }
7956
7957 /**
7958 * Runs precision local-navigation-frame inertial navigation equations.
7959 * NOTE: only the attitude update and specific force frame transformation
7960 * phases are precise.
7961 *
7962 * @param timeInterval time interval between epochs expressed in seconds (s).
7963 * @param oldLatitude previous latitude angle.
7964 * @param oldLongitude previous longitude angle.
7965 * @param oldHeight previous height.
7966 * @param oldC previous body-to-NED coordinate transformation.
7967 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
7968 * resolved along NED-frame axes and expressed in meters per second (m/s).
7969 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
7970 * resolved along NED-frame axes and expressed in meters per second (m/s).
7971 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
7972 * resolved along NED-frame axes and expressed in meters per second (m/s).
7973 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
7974 * resolved along body-frame axes, averaged over time interval and
7975 * expressed in meters per squared second (m/s^2).
7976 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
7977 * resolved along body-frame axes, averaged over time interval and
7978 * expressed in meters per squared second (m/s^2).
7979 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
7980 * resolved along body-frame axes, averaged over time interval and
7981 * expressed in meters per squared second (m/s^2).
7982 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
7983 * resolved along body-frame axes, averaged over time interval and
7984 * expressed in radians per second (rad/s).
7985 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
7986 * resolved along body-frame axes, averaged over time interval and
7987 * expressed in radians per second (rad/s).
7988 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
7989 * resolved along body-frame axes, averaged over time interval and
7990 * expressed in radians per second (rad/s).
7991 * @return estimated NED frame containing new body position, velocity and coordinate
7992 * transformation matrix.
7993 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
7994 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
7995 * body-to-NED-frame coordinate transformation matrix are
7996 * invalid.
7997 */
7998 public NEDFrame navigateAndReturnNew(
7999 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
8000 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
8001 final double fx, final double fy, final double fz,
8002 final double angularRateX, final double angularRateY, final double angularRateZ)
8003 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
8004 return navigateAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
8005 oldVn, oldVe, oldVd, fx, fy, fz, angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
8006 }
8007
8008 /**
8009 * Runs precision local-navigation-frame inertial navigation equations.
8010 * NOTE: only the attitude update and specific force frame transformation
8011 * phases are precise.
8012 *
8013 * @param timeInterval time interval between epochs.
8014 * @param oldLatitude previous latitude angle.
8015 * @param oldLongitude previous longitude angle.
8016 * @param oldHeight previous height.
8017 * @param oldC previous body-to-NED coordinate transformation.
8018 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
8019 * resolved along NED-frame axes and expressed in meters per second (m/s).
8020 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
8021 * resolved along NED-frame axes and expressed in meters per second (m/s).
8022 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
8023 * resolved along NED-frame axes and expressed in meters per second (m/s).
8024 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
8025 * resolved along body-frame axes, averaged over time interval and
8026 * expressed in meters per squared second (m/s^2).
8027 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
8028 * resolved along body-frame axes, averaged over time interval and
8029 * expressed in meters per squared second (m/s^2).
8030 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
8031 * resolved along body-frame axes, averaged over time interval and
8032 * expressed in meters per squared second (m/s^2).
8033 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
8034 * resolved along body-frame axes, averaged over time interval and
8035 * expressed in radians per second (rad/s).
8036 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
8037 * resolved along body-frame axes, averaged over time interval and
8038 * expressed in radians per second (rad/s).
8039 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
8040 * resolved along body-frame axes, averaged over time interval and
8041 * expressed in radians per second (rad/s).
8042 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
8043 * @return estimated NED frame containing new body position, velocity and coordinate
8044 * transformation matrix.
8045 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
8046 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
8047 * body-to-NED-frame coordinate transformation matrix are
8048 * invalid.
8049 */
8050 public NEDFrame navigateAndReturnNew(
8051 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
8052 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
8053 final double fx, final double fy, final double fz,
8054 final double angularRateX, final double angularRateY, final double angularRateZ,
8055 final double accuracyThreshold) throws InertialNavigatorException,
8056 InvalidSourceAndDestinationFrameTypeException {
8057 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
8058 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold);
8059 }
8060
8061 /**
8062 * Runs precision local-navigation-frame inertial navigation equations.
8063 * NOTE: only the attitude update and specific force frame transformation
8064 * phases are precise.
8065 *
8066 * @param timeInterval time interval between epochs.
8067 * @param oldLatitude previous latitude angle.
8068 * @param oldLongitude previous longitude angle.
8069 * @param oldHeight previous height.
8070 * @param oldC previous body-to-NED coordinate transformation.
8071 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
8072 * resolved along NED-frame axes and expressed in meters per second (m/s).
8073 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
8074 * resolved along NED-frame axes and expressed in meters per second (m/s).
8075 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
8076 * resolved along NED-frame axes and expressed in meters per second (m/s).
8077 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
8078 * resolved along body-frame axes, averaged over time interval and
8079 * expressed in meters per squared second (m/s^2).
8080 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
8081 * resolved along body-frame axes, averaged over time interval and
8082 * expressed in meters per squared second (m/s^2).
8083 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
8084 * resolved along body-frame axes, averaged over time interval and
8085 * expressed in meters per squared second (m/s^2).
8086 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
8087 * resolved along body-frame axes, averaged over time interval and
8088 * expressed in radians per second (rad/s).
8089 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
8090 * resolved along body-frame axes, averaged over time interval and
8091 * expressed in radians per second (rad/s).
8092 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
8093 * resolved along body-frame axes, averaged over time interval and
8094 * expressed in radians per second (rad/s).
8095 * @return estimated NED frame containing new body position, velocity and coordinate
8096 * transformation matrix.
8097 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
8098 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
8099 * body-to-NED-frame coordinate transformation matrix are
8100 * invalid.
8101 */
8102 public NEDFrame navigateAndReturnNew(
8103 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
8104 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
8105 final double fx, final double fy, final double fz,
8106 final double angularRateX, final double angularRateY, final double angularRateZ)
8107 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
8108 return navigateAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
8109 fx, fy, fz, angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
8110 }
8111
8112 /**
8113 * Runs precision local-navigation-frame inertial navigation equations.
8114 * NOTE: only the attitude update and specific force frame transformation
8115 * phases are precise.
8116 *
8117 * @param timeInterval time interval between epochs expressed in seconds (s).
8118 * @param oldLatitude previous latitude angle.
8119 * @param oldLongitude previous longitude angle.
8120 * @param oldHeight previous height.
8121 * @param oldC previous body-to-NED coordinate transformation.
8122 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
8123 * along north, east and down axes.
8124 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
8125 * resolved along body-frame axes, averaged over time interval and
8126 * expressed in meters per squared second (m/s^2).
8127 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
8128 * resolved along body-frame axes, averaged over time interval and
8129 * expressed in meters per squared second (m/s^2).
8130 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
8131 * resolved along body-frame axes, averaged over time interval and
8132 * expressed in meters per squared second (m/s^2).
8133 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
8134 * resolved along body-frame axes, averaged over time interval and
8135 * expressed in radians per second (rad/s).
8136 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
8137 * resolved along body-frame axes, averaged over time interval and
8138 * expressed in radians per second (rad/s).
8139 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
8140 * resolved along body-frame axes, averaged over time interval and
8141 * expressed in radians per second (rad/s).
8142 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
8143 * @return estimated NED frame containing new body position, velocity and coordinate
8144 * transformation matrix.
8145 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
8146 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
8147 * body-to-NED-frame coordinate transformation matrix are
8148 * invalid.
8149 */
8150 public NEDFrame navigateAndReturnNew(
8151 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
8152 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
8153 final double fx, final double fy, final double fz,
8154 final double angularRateX, final double angularRateY, final double angularRateZ,
8155 final double accuracyThreshold) throws InertialNavigatorException,
8156 InvalidSourceAndDestinationFrameTypeException {
8157 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity,
8158 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold);
8159 }
8160
8161 /**
8162 * Runs precision local-navigation-frame inertial navigation equations.
8163 * NOTE: only the attitude update and specific force frame transformation
8164 * phases are precise.
8165 *
8166 * @param timeInterval time interval between epochs expressed in seconds (s).
8167 * @param oldLatitude previous latitude angle.
8168 * @param oldLongitude previous longitude angle.
8169 * @param oldHeight previous height.
8170 * @param oldC previous body-to-NED coordinate transformation.
8171 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
8172 * along north, east and down axes.
8173 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
8174 * resolved along body-frame axes, averaged over time interval and
8175 * expressed in meters per squared second (m/s^2).
8176 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
8177 * resolved along body-frame axes, averaged over time interval and
8178 * expressed in meters per squared second (m/s^2).
8179 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
8180 * resolved along body-frame axes, averaged over time interval and
8181 * expressed in meters per squared second (m/s^2).
8182 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
8183 * resolved along body-frame axes, averaged over time interval and
8184 * expressed in radians per second (rad/s).
8185 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
8186 * resolved along body-frame axes, averaged over time interval and
8187 * expressed in radians per second (rad/s).
8188 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
8189 * resolved along body-frame axes, averaged over time interval and
8190 * expressed in radians per second (rad/s).
8191 * @return estimated NED frame containing new body position, velocity and coordinate
8192 * transformation matrix.
8193 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
8194 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
8195 * body-to-NED-frame coordinate transformation matrix are
8196 * invalid.
8197 */
8198 public NEDFrame navigateAndReturnNew(
8199 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
8200 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
8201 final double fx, final double fy, final double fz,
8202 final double angularRateX, final double angularRateY, final double angularRateZ)
8203 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
8204 return navigateAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
8205 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
8206 }
8207
8208 /**
8209 * Runs precision local-navigation-frame inertial navigation equations.
8210 * NOTE: only the attitude update and specific force frame transformation
8211 * phases are precise.
8212 *
8213 * @param timeInterval time interval between epochs.
8214 * @param oldLatitude previous latitude angle.
8215 * @param oldLongitude previous longitude angle.
8216 * @param oldHeight previous height.
8217 * @param oldC previous body-to-NED coordinate transformation.
8218 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
8219 * along north, east and down axes.
8220 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
8221 * resolved along body-frame axes, averaged over time interval and
8222 * expressed in meters per squared second (m/s^2).
8223 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
8224 * resolved along body-frame axes, averaged over time interval and
8225 * expressed in meters per squared second (m/s^2).
8226 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
8227 * resolved along body-frame axes, averaged over time interval and
8228 * expressed in meters per squared second (m/s^2).
8229 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
8230 * resolved along body-frame axes, averaged over time interval and
8231 * expressed in radians per second (rad/s).
8232 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
8233 * resolved along body-frame axes, averaged over time interval and
8234 * expressed in radians per second (rad/s).
8235 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
8236 * resolved along body-frame axes, averaged over time interval and
8237 * expressed in radians per second (rad/s).
8238 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
8239 * @return estimated NED frame containing new body position, velocity and coordinate
8240 * transformation matrix.
8241 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
8242 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
8243 * body-to-NED-frame coordinate transformation matrix are
8244 * invalid.
8245 */
8246 public NEDFrame navigateAndReturnNew(
8247 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
8248 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
8249 final double fx, final double fy, final double fz,
8250 final double angularRateX, final double angularRateY, final double angularRateZ,
8251 final double accuracyThreshold) throws InertialNavigatorException,
8252 InvalidSourceAndDestinationFrameTypeException {
8253 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity,
8254 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold);
8255 }
8256
8257 /**
8258 * Runs precision local-navigation-frame inertial navigation equations.
8259 * NOTE: only the attitude update and specific force frame transformation
8260 * phases are precise.
8261 *
8262 * @param timeInterval time interval between epochs.
8263 * @param oldLatitude previous latitude angle.
8264 * @param oldLongitude previous longitude angle.
8265 * @param oldHeight previous height.
8266 * @param oldC previous body-to-NED coordinate transformation.
8267 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
8268 * along north, east and down axes.
8269 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
8270 * resolved along body-frame axes, averaged over time interval and
8271 * expressed in meters per squared second (m/s^2).
8272 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
8273 * resolved along body-frame axes, averaged over time interval and
8274 * expressed in meters per squared second (m/s^2).
8275 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
8276 * resolved along body-frame axes, averaged over time interval and
8277 * expressed in meters per squared second (m/s^2).
8278 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
8279 * resolved along body-frame axes, averaged over time interval and
8280 * expressed in radians per second (rad/s).
8281 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
8282 * resolved along body-frame axes, averaged over time interval and
8283 * expressed in radians per second (rad/s).
8284 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
8285 * resolved along body-frame axes, averaged over time interval and
8286 * expressed in radians per second (rad/s).
8287 * @return estimated NED frame containing new body position, velocity and coordinate
8288 * transformation matrix.
8289 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
8290 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
8291 * body-to-NED-frame coordinate transformation matrix are
8292 * invalid.
8293 */
8294 public NEDFrame navigateAndReturnNew(
8295 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
8296 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
8297 final double fx, final double fy, final double fz,
8298 final double angularRateX, final double angularRateY, final double angularRateZ)
8299 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
8300 return navigateAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
8301 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
8302 }
8303
8304 /**
8305 * Runs precision local-navigation-frame inertial navigation equations.
8306 * NOTE: only the attitude update and specific force frame transformation
8307 * phases are precise.
8308 *
8309 * @param timeInterval time interval between epochs expressed in seconds (s).
8310 * @param oldLatitude previous latitude angle.
8311 * @param oldLongitude previous longitude angle.
8312 * @param oldHeight previous height.
8313 * @param oldC previous body-to-NED coordinate transformation.
8314 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
8315 * resolved along NED-frame axes and expressed in meters per second (m/s).
8316 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
8317 * resolved along NED-frame axes and expressed in meters per second (m/s).
8318 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
8319 * resolved along NED-frame axes and expressed in meters per second (m/s).
8320 * @param kinematics body kinematics containing specific forces and angular rates applied to
8321 * the body.
8322 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
8323 * @return estimated NED frame containing new body position, velocity and coordinate
8324 * transformation matrix.
8325 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
8326 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
8327 * body-to-NED-frame coordinate transformation matrix are
8328 * invalid.
8329 */
8330 public NEDFrame navigateAndReturnNew(
8331 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
8332 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
8333 final BodyKinematics kinematics, final double accuracyThreshold) throws InertialNavigatorException,
8334 InvalidSourceAndDestinationFrameTypeException {
8335 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
8336 kinematics, accuracyThreshold);
8337 }
8338
8339 /**
8340 * Runs precision local-navigation-frame inertial navigation equations.
8341 * NOTE: only the attitude update and specific force frame transformation
8342 * phases are precise.
8343 *
8344 * @param timeInterval time interval between epochs expressed in seconds (s).
8345 * @param oldLatitude previous latitude angle.
8346 * @param oldLongitude previous longitude angle.
8347 * @param oldHeight previous height.
8348 * @param oldC previous body-to-NED coordinate transformation.
8349 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
8350 * resolved along NED-frame axes and expressed in meters per second (m/s).
8351 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
8352 * resolved along NED-frame axes and expressed in meters per second (m/s).
8353 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
8354 * resolved along NED-frame axes and expressed in meters per second (m/s).
8355 * @param kinematics body kinematics containing specific forces and angular rates applied to
8356 * the body.
8357 * @return estimated NED frame containing new body position, velocity and coordinate
8358 * transformation matrix.
8359 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
8360 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
8361 * body-to-NED-frame coordinate transformation matrix are
8362 * invalid.
8363 */
8364 public NEDFrame navigateAndReturnNew(
8365 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
8366 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
8367 final BodyKinematics kinematics) throws InertialNavigatorException,
8368 InvalidSourceAndDestinationFrameTypeException {
8369 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
8370 kinematics, DEFAULT_ACCURACY_THRESHOLD);
8371 }
8372
8373 /**
8374 * Runs precision local-navigation-frame inertial navigation equations.
8375 * NOTE: only the attitude update and specific force frame transformation
8376 * phases are precise.
8377 *
8378 * @param timeInterval time interval between epochs.
8379 * @param oldLatitude previous latitude angle.
8380 * @param oldLongitude previous longitude angle.
8381 * @param oldHeight previous height.
8382 * @param oldC previous body-to-NED coordinate transformation.
8383 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
8384 * resolved along NED-frame axes and expressed in meters per second (m/s).
8385 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
8386 * resolved along NED-frame axes and expressed in meters per second (m/s).
8387 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
8388 * resolved along NED-frame axes and expressed in meters per second (m/s).
8389 * @param kinematics body kinematics containing specific forces and angular rates applied to
8390 * the body.
8391 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
8392 * @return estimated NED frame containing new body position, velocity and coordinate
8393 * transformation matrix.
8394 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
8395 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
8396 * body-to-NED-frame coordinate transformation matrix are
8397 * invalid.
8398 */
8399 public NEDFrame navigateAndReturnNew(
8400 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
8401 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
8402 final BodyKinematics kinematics, final double accuracyThreshold) throws InertialNavigatorException,
8403 InvalidSourceAndDestinationFrameTypeException {
8404 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
8405 kinematics, accuracyThreshold);
8406 }
8407
8408 /**
8409 * Runs precision local-navigation-frame inertial navigation equations.
8410 * NOTE: only the attitude update and specific force frame transformation
8411 * phases are precise.
8412 *
8413 * @param timeInterval time interval between epochs.
8414 * @param oldLatitude previous latitude angle.
8415 * @param oldLongitude previous longitude angle.
8416 * @param oldHeight previous height.
8417 * @param oldC previous body-to-NED coordinate transformation.
8418 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
8419 * resolved along NED-frame axes and expressed in meters per second (m/s).
8420 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
8421 * resolved along NED-frame axes and expressed in meters per second (m/s).
8422 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
8423 * resolved along NED-frame axes and expressed in meters per second (m/s).
8424 * @param kinematics body kinematics containing specific forces and angular rates applied to
8425 * the body.
8426 * @return estimated NED frame containing new body position, velocity and coordinate
8427 * transformation matrix.
8428 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
8429 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
8430 * body-to-NED-frame coordinate transformation matrix are
8431 * invalid.
8432 */
8433 public NEDFrame navigateAndReturnNew(
8434 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
8435 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
8436 final BodyKinematics kinematics) throws InertialNavigatorException,
8437 InvalidSourceAndDestinationFrameTypeException {
8438 return navigateAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
8439 kinematics, DEFAULT_ACCURACY_THRESHOLD);
8440 }
8441
8442 /**
8443 * Runs precision local-navigation-frame inertial navigation equations.
8444 * NOTE: only the attitude update and specific force frame transformation
8445 * phases are precise.
8446 *
8447 * @param timeInterval time interval between epochs expressed in seconds (s).
8448 * @param oldLatitude previous latitude angle.
8449 * @param oldLongitude previous longitude angle.
8450 * @param oldHeight previous height.
8451 * @param oldC previous body-to-NED coordinate transformation.
8452 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
8453 * along north, east and down axes.
8454 * @param kinematics body kinematics containing specific forces and angular rates applied to
8455 * the body.
8456 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
8457 * @return estimated NED frame containing new body position, velocity and coordinate
8458 * transformation matrix.
8459 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
8460 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
8461 * body-to-NED-frame coordinate transformation matrix are
8462 * invalid.
8463 */
8464 public NEDFrame navigateAndReturnNew(
8465 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
8466 final CoordinateTransformation oldC, final NEDVelocity oldVelocity, final BodyKinematics kinematics,
8467 final double accuracyThreshold) throws InertialNavigatorException,
8468 InvalidSourceAndDestinationFrameTypeException {
8469 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity,
8470 kinematics, accuracyThreshold);
8471 }
8472
8473 /**
8474 * Runs precision local-navigation-frame inertial navigation equations.
8475 * NOTE: only the attitude update and specific force frame transformation
8476 * phases are precise.
8477 *
8478 * @param timeInterval time interval between epochs expressed in seconds (s).
8479 * @param oldLatitude previous latitude angle.
8480 * @param oldLongitude previous longitude angle.
8481 * @param oldHeight previous height.
8482 * @param oldC previous body-to-NED coordinate transformation.
8483 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
8484 * along north, east and down axes.
8485 * @param kinematics body kinematics containing specific forces and angular rates applied to
8486 * the body.
8487 * @return estimated NED frame containing new body position, velocity and coordinate
8488 * transformation matrix.
8489 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
8490 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
8491 * body-to-NED-frame coordinate transformation matrix are
8492 * invalid.
8493 */
8494 public NEDFrame navigateAndReturnNew(
8495 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
8496 final CoordinateTransformation oldC, final NEDVelocity oldVelocity, final BodyKinematics kinematics)
8497 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
8498 return navigateAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, kinematics,
8499 DEFAULT_ACCURACY_THRESHOLD);
8500 }
8501
8502 /**
8503 * Runs precision local-navigation-frame inertial navigation equations.
8504 * NOTE: only the attitude update and specific force frame transformation
8505 * phases are precise.
8506 *
8507 * @param timeInterval time interval between epochs.
8508 * @param oldLatitude previous latitude angle.
8509 * @param oldLongitude previous longitude angle.
8510 * @param oldHeight previous height.
8511 * @param oldC previous body-to-NED coordinate transformation.
8512 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
8513 * along north, east and down axes.
8514 * @param kinematics body kinematics containing specific forces and angular rates applied to
8515 * the body.
8516 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
8517 * @return estimated NED frame containing new body position, velocity and coordinate
8518 * transformation matrix.
8519 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
8520 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
8521 * body-to-NED-frame coordinate transformation matrix are
8522 * invalid.
8523 */
8524 public NEDFrame navigateAndReturnNew(
8525 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
8526 final CoordinateTransformation oldC, final NEDVelocity oldVelocity, final BodyKinematics kinematics,
8527 final double accuracyThreshold) throws InertialNavigatorException,
8528 InvalidSourceAndDestinationFrameTypeException {
8529 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity,
8530 kinematics, accuracyThreshold);
8531 }
8532
8533 /**
8534 * Runs precision local-navigation-frame inertial navigation equations.
8535 * NOTE: only the attitude update and specific force frame transformation
8536 * phases are precise.
8537 *
8538 * @param timeInterval time interval between epochs.
8539 * @param oldLatitude previous latitude angle.
8540 * @param oldLongitude previous longitude angle.
8541 * @param oldHeight previous height.
8542 * @param oldC previous body-to-NED coordinate transformation.
8543 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
8544 * along north, east and down axes.
8545 * @param kinematics body kinematics containing specific forces and angular rates applied to
8546 * the body.
8547 * @return estimated NED frame containing new body position, velocity and coordinate
8548 * transformation matrix.
8549 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
8550 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
8551 * body-to-NED-frame coordinate transformation matrix are
8552 * invalid.
8553 */
8554 public NEDFrame navigateAndReturnNew(
8555 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
8556 final CoordinateTransformation oldC, final NEDVelocity oldVelocity, final BodyKinematics kinematics)
8557 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
8558 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity,
8559 kinematics, DEFAULT_ACCURACY_THRESHOLD);
8560 }
8561
8562 /**
8563 * Runs precision local-navigation-frame inertial navigation equations.
8564 * NOTE: only the attitude update and specific force frame transformation
8565 * phases are precise.
8566 *
8567 * @param timeInterval time interval between epochs expressed in seconds (s).
8568 * @param oldLatitude previous latitude expressed in radians (rad).
8569 * @param oldLongitude previous longitude expressed in radians (rad).
8570 * @param oldHeight previous height expressed in meters (m).
8571 * @param oldC previous body-to-NED coordinate transformation.
8572 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
8573 * resolved along NED-frame axes.
8574 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
8575 * resolved along NED-frame axes.
8576 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
8577 * resolved along NED-frame axes.
8578 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
8579 * resolved along body-frame axes, averaged over time interval and
8580 * expressed in meters per squared second (m/s^2).
8581 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
8582 * resolved along body-frame axes, averaged over time interval and
8583 * expressed in meters per squared second (m/s^2).
8584 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
8585 * resolved along body-frame axes, averaged over time interval and
8586 * expressed in meters per squared second (m/s^2).
8587 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
8588 * resolved along body-frame axes, averaged over time interval and
8589 * expressed in radians per second (rad/s).
8590 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
8591 * resolved along body-frame axes, averaged over time interval and
8592 * expressed in radians per second (rad/s).
8593 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
8594 * resolved along body-frame axes, averaged over time interval and
8595 * expressed in radians per second (rad/s).
8596 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
8597 * @return estimated NED frame containing new body position, velocity and coordinate
8598 * transformation matrix.
8599 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
8600 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
8601 * body-to-NED-frame coordinate transformation matrix are
8602 * invalid.
8603 */
8604 public NEDFrame navigateAndReturnNew(
8605 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
8606 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
8607 final double fx, final double fy, final double fz,
8608 final double angularRateX, final double angularRateY, final double angularRateZ,
8609 final double accuracyThreshold)
8610 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
8611 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
8612 oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
8613 accuracyThreshold);
8614 }
8615
8616 /**
8617 * Runs precision local-navigation-frame inertial navigation equations.
8618 * NOTE: only the attitude update and specific force frame transformation
8619 * phases are precise.
8620 *
8621 * @param timeInterval time interval between epochs expressed in seconds (s).
8622 * @param oldLatitude previous latitude expressed in radians (rad).
8623 * @param oldLongitude previous longitude expressed in radians (rad).
8624 * @param oldHeight previous height expressed in meters (m).
8625 * @param oldC previous body-to-NED coordinate transformation.
8626 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
8627 * resolved along NED-frame axes.
8628 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
8629 * resolved along NED-frame axes.
8630 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
8631 * resolved along NED-frame axes.
8632 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
8633 * resolved along body-frame axes, averaged over time interval and
8634 * expressed in meters per squared second (m/s^2).
8635 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
8636 * resolved along body-frame axes, averaged over time interval and
8637 * expressed in meters per squared second (m/s^2).
8638 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
8639 * resolved along body-frame axes, averaged over time interval and
8640 * expressed in meters per squared second (m/s^2).
8641 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
8642 * resolved along body-frame axes, averaged over time interval and
8643 * expressed in radians per second (rad/s).
8644 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
8645 * resolved along body-frame axes, averaged over time interval and
8646 * expressed in radians per second (rad/s).
8647 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
8648 * resolved along body-frame axes, averaged over time interval and
8649 * expressed in radians per second (rad/s).
8650 * @return estimated NED frame containing new body position, velocity and coordinate
8651 * transformation matrix.
8652 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
8653 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
8654 * body-to-NED-frame coordinate transformation matrix are
8655 * invalid.
8656 */
8657 public NEDFrame navigateAndReturnNew(
8658 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
8659 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
8660 final double fx, final double fy, final double fz,
8661 final double angularRateX, final double angularRateY, final double angularRateZ)
8662 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
8663 return navigateAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
8664 oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
8665 DEFAULT_ACCURACY_THRESHOLD);
8666 }
8667
8668 /**
8669 * Runs precision local-navigation-frame inertial navigation equations.
8670 * NOTE: only the attitude update and specific force frame transformation
8671 * phases are precise.
8672 *
8673 * @param timeInterval time interval between epochs.
8674 * @param oldLatitude previous latitude expressed in radians (rad).
8675 * @param oldLongitude previous longitude expressed in radians (rad).
8676 * @param oldHeight previous height expressed in meters (m).
8677 * @param oldC previous body-to-NED coordinate transformation.
8678 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
8679 * resolved along NED-frame axes.
8680 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
8681 * resolved along NED-frame axes.
8682 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
8683 * resolved along NED-frame axes.
8684 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
8685 * resolved along body-frame axes, averaged over time interval and
8686 * expressed in meters per squared second (m/s^2).
8687 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
8688 * resolved along body-frame axes, averaged over time interval and
8689 * expressed in meters per squared second (m/s^2).
8690 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
8691 * resolved along body-frame axes, averaged over time interval and
8692 * expressed in meters per squared second (m/s^2).
8693 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
8694 * resolved along body-frame axes, averaged over time interval and
8695 * expressed in radians per second (rad/s).
8696 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
8697 * resolved along body-frame axes, averaged over time interval and
8698 * expressed in radians per second (rad/s).
8699 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
8700 * resolved along body-frame axes, averaged over time interval and
8701 * expressed in radians per second (rad/s).
8702 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
8703 * @return estimated NED frame containing new body position, velocity and coordinate
8704 * transformation matrix.
8705 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
8706 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
8707 * body-to-NED-frame coordinate transformation matrix are
8708 * invalid.
8709 */
8710 public NEDFrame navigateAndReturnNew(
8711 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
8712 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
8713 final double fx, final double fy, final double fz,
8714 final double angularRateX, final double angularRateY, final double angularRateZ,
8715 final double accuracyThreshold) throws InertialNavigatorException,
8716 InvalidSourceAndDestinationFrameTypeException {
8717 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
8718 oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
8719 accuracyThreshold);
8720 }
8721
8722 /**
8723 * Runs precision local-navigation-frame inertial navigation equations.
8724 * NOTE: only the attitude update and specific force frame transformation
8725 * phases are precise.
8726 *
8727 * @param timeInterval time interval between epochs.
8728 * @param oldLatitude previous latitude expressed in radians (rad).
8729 * @param oldLongitude previous longitude expressed in radians (rad).
8730 * @param oldHeight previous height expressed in meters (m).
8731 * @param oldC previous body-to-NED coordinate transformation.
8732 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
8733 * resolved along NED-frame axes.
8734 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
8735 * resolved along NED-frame axes.
8736 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
8737 * resolved along NED-frame axes.
8738 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
8739 * resolved along body-frame axes, averaged over time interval and
8740 * expressed in meters per squared second (m/s^2).
8741 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
8742 * resolved along body-frame axes, averaged over time interval and
8743 * expressed in meters per squared second (m/s^2).
8744 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
8745 * resolved along body-frame axes, averaged over time interval and
8746 * expressed in meters per squared second (m/s^2).
8747 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
8748 * resolved along body-frame axes, averaged over time interval and
8749 * expressed in radians per second (rad/s).
8750 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
8751 * resolved along body-frame axes, averaged over time interval and
8752 * expressed in radians per second (rad/s).
8753 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
8754 * resolved along body-frame axes, averaged over time interval and
8755 * expressed in radians per second (rad/s).
8756 * @return estimated NED frame containing new body position, velocity and coordinate
8757 * transformation matrix.
8758 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
8759 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
8760 * body-to-NED-frame coordinate transformation matrix are
8761 * invalid.
8762 */
8763 public NEDFrame navigateAndReturnNew(
8764 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
8765 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
8766 final double fx, final double fy, final double fz,
8767 final double angularRateX, final double angularRateY, final double angularRateZ)
8768 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
8769 return navigateAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
8770 oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
8771 DEFAULT_ACCURACY_THRESHOLD);
8772 }
8773
8774 /**
8775 * Runs precision local-navigation-frame inertial navigation equations.
8776 * NOTE: only the attitude update and specific force frame transformation
8777 * phases are precise.
8778 *
8779 * @param timeInterval time interval between epochs expressed in seconds (s).
8780 * @param oldPosition previous curvilinear position expressed in terms of latitude,
8781 * longitude and height.
8782 * @param oldC previous body-to-NED coordinate transformation.
8783 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
8784 * resolved along NED-frame axes.
8785 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
8786 * resolved along NED-frame axes.
8787 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
8788 * resolved along NED-frame axes.
8789 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
8790 * resolved along body-frame axes, averaged over time interval and
8791 * expressed in meters per squared second (m/s^2).
8792 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
8793 * resolved along body-frame axes, averaged over time interval and
8794 * expressed in meters per squared second (m/s^2).
8795 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
8796 * resolved along body-frame axes, averaged over time interval and
8797 * expressed in meters per squared second (m/s^2).
8798 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
8799 * resolved along body-frame axes, averaged over time interval and
8800 * expressed in radians per second (rad/s).
8801 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
8802 * resolved along body-frame axes, averaged over time interval and
8803 * expressed in radians per second (rad/s).
8804 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
8805 * resolved along body-frame axes, averaged over time interval and
8806 * expressed in radians per second (rad/s).
8807 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
8808 * @return estimated NED frame containing new body position, velocity and coordinate
8809 * transformation matrix.
8810 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
8811 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
8812 * body-to-NED-frame coordinate transformation matrix are
8813 * invalid.
8814 */
8815 public NEDFrame navigateAndReturnNew(
8816 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
8817 final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
8818 final double fx, final double fy, final double fz,
8819 final double angularRateX, final double angularRateY, final double angularRateZ,
8820 final double accuracyThreshold) throws InertialNavigatorException,
8821 InvalidSourceAndDestinationFrameTypeException {
8822 return navigateNEDAndReturnNew(timeInterval, oldPosition, oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
8823 angularRateX, angularRateY, angularRateZ, accuracyThreshold);
8824 }
8825
8826 /**
8827 * Runs precision local-navigation-frame inertial navigation equations.
8828 * NOTE: only the attitude update and specific force frame transformation
8829 * phases are precise.
8830 *
8831 * @param timeInterval time interval between epochs expressed in seconds (s).
8832 * @param oldPosition previous curvilinear position expressed in terms of latitude,
8833 * longitude and height.
8834 * @param oldC previous body-to-NED coordinate transformation.
8835 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
8836 * resolved along NED-frame axes.
8837 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
8838 * resolved along NED-frame axes.
8839 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
8840 * resolved along NED-frame axes.
8841 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
8842 * resolved along body-frame axes, averaged over time interval and
8843 * expressed in meters per squared second (m/s^2).
8844 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
8845 * resolved along body-frame axes, averaged over time interval and
8846 * expressed in meters per squared second (m/s^2).
8847 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
8848 * resolved along body-frame axes, averaged over time interval and
8849 * expressed in meters per squared second (m/s^2).
8850 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
8851 * resolved along body-frame axes, averaged over time interval and
8852 * expressed in radians per second (rad/s).
8853 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
8854 * resolved along body-frame axes, averaged over time interval and
8855 * expressed in radians per second (rad/s).
8856 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
8857 * resolved along body-frame axes, averaged over time interval and
8858 * expressed in radians per second (rad/s).
8859 * @return estimated NED frame containing new body position, velocity and coordinate
8860 * transformation matrix.
8861 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
8862 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
8863 * body-to-NED-frame coordinate transformation matrix are
8864 * invalid.
8865 */
8866 public NEDFrame navigateAndReturnNew(
8867 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
8868 final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
8869 final double fx, final double fy, final double fz,
8870 final double angularRateX, final double angularRateY, final double angularRateZ)
8871 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
8872 return navigateAndReturnNew(timeInterval, oldPosition, oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
8873 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
8874 }
8875
8876 /**
8877 * Runs precision local-navigation-frame inertial navigation equations.
8878 * NOTE: only the attitude update and specific force frame transformation
8879 * phases are precise.
8880 *
8881 * @param timeInterval time interval between epochs.
8882 * @param oldPosition previous curvilinear position expressed in terms of latitude,
8883 * longitude and height.
8884 * @param oldC previous body-to-NED coordinate transformation.
8885 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
8886 * resolved along NED-frame axes.
8887 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
8888 * resolved along NED-frame axes.
8889 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
8890 * resolved along NED-frame axes.
8891 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
8892 * resolved along body-frame axes, averaged over time interval and
8893 * expressed in meters per squared second (m/s^2).
8894 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
8895 * resolved along body-frame axes, averaged over time interval and
8896 * expressed in meters per squared second (m/s^2).
8897 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
8898 * resolved along body-frame axes, averaged over time interval and
8899 * expressed in meters per squared second (m/s^2).
8900 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
8901 * resolved along body-frame axes, averaged over time interval and
8902 * expressed in radians per second (rad/s).
8903 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
8904 * resolved along body-frame axes, averaged over time interval and
8905 * expressed in radians per second (rad/s).
8906 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
8907 * resolved along body-frame axes, averaged over time interval and
8908 * expressed in radians per second (rad/s).
8909 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
8910 * @return estimated NED frame containing new body position, velocity and coordinate
8911 * transformation matrix.
8912 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
8913 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
8914 * body-to-NED-frame coordinate transformation matrix are
8915 * invalid.
8916 */
8917 public NEDFrame navigateAndReturnNew(
8918 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
8919 final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
8920 final double fx, final double fy, final double fz,
8921 final double angularRateX, final double angularRateY, final double angularRateZ,
8922 final double accuracyThreshold) throws InertialNavigatorException,
8923 InvalidSourceAndDestinationFrameTypeException {
8924 return navigateNEDAndReturnNew(timeInterval, oldPosition, oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
8925 angularRateX, angularRateY, angularRateZ, accuracyThreshold);
8926 }
8927
8928 /**
8929 * Runs precision local-navigation-frame inertial navigation equations.
8930 * NOTE: only the attitude update and specific force frame transformation
8931 * phases are precise.
8932 *
8933 * @param timeInterval time interval between epochs.
8934 * @param oldPosition previous curvilinear position expressed in terms of latitude,
8935 * longitude and height.
8936 * @param oldC previous body-to-NED coordinate transformation.
8937 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
8938 * resolved along NED-frame axes.
8939 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
8940 * resolved along NED-frame axes.
8941 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
8942 * resolved along NED-frame axes.
8943 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
8944 * resolved along body-frame axes, averaged over time interval and
8945 * expressed in meters per squared second (m/s^2).
8946 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
8947 * resolved along body-frame axes, averaged over time interval and
8948 * expressed in meters per squared second (m/s^2).
8949 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
8950 * resolved along body-frame axes, averaged over time interval and
8951 * expressed in meters per squared second (m/s^2).
8952 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
8953 * resolved along body-frame axes, averaged over time interval and
8954 * expressed in radians per second (rad/s).
8955 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
8956 * resolved along body-frame axes, averaged over time interval and
8957 * expressed in radians per second (rad/s).
8958 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
8959 * resolved along body-frame axes, averaged over time interval and
8960 * expressed in radians per second (rad/s).
8961 * @return estimated NED frame containing new body position, velocity and coordinate
8962 * transformation matrix.
8963 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
8964 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
8965 * body-to-NED-frame coordinate transformation matrix are
8966 * invalid.
8967 */
8968 public NEDFrame navigateAndReturnNew(
8969 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
8970 final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
8971 final double fx, final double fy, final double fz,
8972 final double angularRateX, final double angularRateY, final double angularRateZ)
8973 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
8974 return navigateAndReturnNew(timeInterval, oldPosition, oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
8975 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
8976 }
8977
8978 /**
8979 * Runs precision local-navigation-frame inertial navigation equations.
8980 * NOTE: only the attitude update and specific force frame transformation
8981 * phases are precise.
8982 *
8983 * @param timeInterval time interval between epochs expressed in seconds (s).
8984 * @param oldLatitude previous latitude expressed in radians (rad).
8985 * @param oldLongitude previous longitude expressed in radians (rad).
8986 * @param oldHeight previous height expressed in meters (m).
8987 * @param oldC previous body-to-NED coordinate transformation.
8988 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
8989 * resolved along NED-frame axes.
8990 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
8991 * resolved along NED-frame axes.
8992 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
8993 * resolved along NED-frame axes.
8994 * @param kinematics body kinematics containing specific forces and angular rates applied to
8995 * the body.
8996 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
8997 * @return estimated NED frame containing new body position, velocity and coordinate
8998 * transformation matrix.
8999 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
9000 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
9001 * body-to-NED-frame coordinate transformation matrix are
9002 * invalid.
9003 */
9004 public NEDFrame navigateAndReturnNew(
9005 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
9006 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
9007 final BodyKinematics kinematics, final double accuracyThreshold) throws InertialNavigatorException,
9008 InvalidSourceAndDestinationFrameTypeException {
9009 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
9010 oldSpeedN, oldSpeedE, oldSpeedD, kinematics, accuracyThreshold);
9011 }
9012
9013 /**
9014 * Runs precision local-navigation-frame inertial navigation equations.
9015 * NOTE: only the attitude update and specific force frame transformation
9016 * phases are precise.
9017 *
9018 * @param timeInterval time interval between epochs expressed in seconds (s).
9019 * @param oldLatitude previous latitude expressed in radians (rad).
9020 * @param oldLongitude previous longitude expressed in radians (rad).
9021 * @param oldHeight previous height expressed in meters (m).
9022 * @param oldC previous body-to-NED coordinate transformation.
9023 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
9024 * resolved along NED-frame axes.
9025 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
9026 * resolved along NED-frame axes.
9027 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
9028 * resolved along NED-frame axes.
9029 * @param kinematics body kinematics containing specific forces and angular rates applied to
9030 * the body.
9031 * @return estimated NED frame containing new body position, velocity and coordinate
9032 * transformation matrix.
9033 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
9034 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
9035 * body-to-NED-frame coordinate transformation matrix are
9036 * invalid.
9037 */
9038 public NEDFrame navigateAndReturnNew(
9039 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
9040 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
9041 final BodyKinematics kinematics) throws InertialNavigatorException,
9042 InvalidSourceAndDestinationFrameTypeException {
9043 return navigateAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
9044 oldSpeedN, oldSpeedE, oldSpeedD, kinematics, DEFAULT_ACCURACY_THRESHOLD);
9045 }
9046
9047 /**
9048 * Runs precision local-navigation-frame inertial navigation equations.
9049 * NOTE: only the attitude update and specific force frame transformation
9050 * phases are precise.
9051 *
9052 * @param timeInterval time interval between epochs.
9053 * @param oldLatitude previous latitude expressed in radians (rad).
9054 * @param oldLongitude previous longitude expressed in radians (rad).
9055 * @param oldHeight previous height expressed in meters (m).
9056 * @param oldC previous body-to-NED coordinate transformation.
9057 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
9058 * resolved along NED-frame axes.
9059 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
9060 * resolved along NED-frame axes.
9061 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
9062 * resolved along NED-frame axes.
9063 * @param kinematics body kinematics containing specific forces and angular rates applied to
9064 * the body.
9065 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
9066 * @return estimated NED frame containing new body position, velocity and coordinate
9067 * transformation matrix.
9068 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
9069 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
9070 * body-to-NED-frame coordinate transformation matrix are
9071 * invalid.
9072 */
9073 public NEDFrame navigateAndReturnNew(
9074 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
9075 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
9076 final BodyKinematics kinematics, final double accuracyThreshold) throws InertialNavigatorException,
9077 InvalidSourceAndDestinationFrameTypeException {
9078 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
9079 oldSpeedN, oldSpeedE, oldSpeedD, kinematics, accuracyThreshold);
9080 }
9081
9082 /**
9083 * Runs precision local-navigation-frame inertial navigation equations.
9084 * NOTE: only the attitude update and specific force frame transformation
9085 * phases are precise.
9086 *
9087 * @param timeInterval time interval between epochs.
9088 * @param oldLatitude previous latitude expressed in radians (rad).
9089 * @param oldLongitude previous longitude expressed in radians (rad).
9090 * @param oldHeight previous height expressed in meters (m).
9091 * @param oldC previous body-to-NED coordinate transformation.
9092 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
9093 * resolved along NED-frame axes.
9094 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
9095 * resolved along NED-frame axes.
9096 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
9097 * resolved along NED-frame axes.
9098 * @param kinematics body kinematics containing specific forces and angular rates applied to
9099 * the body.
9100 * @return estimated NED frame containing new body position, velocity and coordinate
9101 * transformation matrix.
9102 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
9103 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
9104 * body-to-NED-frame coordinate transformation matrix are
9105 * invalid.
9106 */
9107 public NEDFrame navigateAndReturnNew(
9108 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
9109 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
9110 final BodyKinematics kinematics) throws InertialNavigatorException,
9111 InvalidSourceAndDestinationFrameTypeException {
9112 return navigateAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
9113 oldSpeedN, oldSpeedE, oldSpeedD, kinematics, DEFAULT_ACCURACY_THRESHOLD);
9114 }
9115
9116 /**
9117 * Runs precision local-navigation-frame inertial navigation equations.
9118 * NOTE: only the attitude update and specific force frame transformation
9119 * phases are precise.
9120 *
9121 * @param timeInterval time interval between epochs expressed in seconds (s).
9122 * @param oldPosition previous curvilinear position expressed in terms of latitude,
9123 * longitude and height.
9124 * @param oldC previous body-to-NED coordinate transformation.
9125 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
9126 * resolved along NED-frame axes.
9127 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
9128 * resolved along NED-frame axes.
9129 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
9130 * resolved along NED-frame axes.
9131 * @param kinematics body kinematics containing specific forces and angular rates applied to
9132 * the body.
9133 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
9134 * @return estimated NED frame containing new body position, velocity and coordinate
9135 * transformation matrix.
9136 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
9137 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
9138 * body-to-NED-frame coordinate transformation matrix are
9139 * invalid.
9140 */
9141 public NEDFrame navigateAndReturnNew(
9142 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
9143 final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD, final BodyKinematics kinematics,
9144 final double accuracyThreshold) throws InertialNavigatorException,
9145 InvalidSourceAndDestinationFrameTypeException {
9146 return navigateNEDAndReturnNew(timeInterval, oldPosition, oldC, oldSpeedN, oldSpeedE, oldSpeedD, kinematics,
9147 accuracyThreshold);
9148 }
9149
9150 /**
9151 * Runs precision local-navigation-frame inertial navigation equations.
9152 * NOTE: only the attitude update and specific force frame transformation
9153 * phases are precise.
9154 *
9155 * @param timeInterval time interval between epochs expressed in seconds (s).
9156 * @param oldPosition previous curvilinear position expressed in terms of latitude,
9157 * longitude and height.
9158 * @param oldC previous body-to-NED coordinate transformation.
9159 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
9160 * resolved along NED-frame axes.
9161 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
9162 * resolved along NED-frame axes.
9163 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
9164 * resolved along NED-frame axes.
9165 * @param kinematics body kinematics containing specific forces and angular rates applied to
9166 * the body.
9167 * @return estimated NED frame containing new body position, velocity and coordinate
9168 * transformation matrix.
9169 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
9170 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
9171 * body-to-NED-frame coordinate transformation matrix are
9172 * invalid.
9173 */
9174 public NEDFrame navigateAndReturnNew(
9175 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
9176 final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD, final BodyKinematics kinematics)
9177 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
9178 return navigateAndReturnNew(timeInterval, oldPosition, oldC, oldSpeedN, oldSpeedE, oldSpeedD, kinematics,
9179 DEFAULT_ACCURACY_THRESHOLD);
9180 }
9181
9182 /**
9183 * Runs precision local-navigation-frame inertial navigation equations.
9184 * NOTE: only the attitude update and specific force frame transformation
9185 * phases are precise.
9186 *
9187 * @param timeInterval time interval between epochs.
9188 * @param oldPosition previous curvilinear position expressed in terms of latitude,
9189 * longitude and height.
9190 * @param oldC previous body-to-NED coordinate transformation.
9191 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
9192 * resolved along NED-frame axes.
9193 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
9194 * resolved along NED-frame axes.
9195 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
9196 * resolved along NED-frame axes.
9197 * @param kinematics body kinematics containing specific forces and angular rates applied to
9198 * the body.
9199 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
9200 * @return estimated NED frame containing new body position, velocity and coordinate
9201 * transformation matrix.
9202 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
9203 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
9204 * body-to-NED-frame coordinate transformation matrix are
9205 * invalid.
9206 */
9207 public NEDFrame navigateAndReturnNew(
9208 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
9209 final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD, final BodyKinematics kinematics,
9210 final double accuracyThreshold) throws InertialNavigatorException,
9211 InvalidSourceAndDestinationFrameTypeException {
9212 return navigateNEDAndReturnNew(timeInterval, oldPosition, oldC, oldSpeedN, oldSpeedE, oldSpeedD, kinematics,
9213 accuracyThreshold);
9214 }
9215
9216 /**
9217 * Runs precision local-navigation-frame inertial navigation equations.
9218 * NOTE: only the attitude update and specific force frame transformation
9219 * phases are precise.
9220 *
9221 * @param timeInterval time interval between epochs.
9222 * @param oldPosition previous curvilinear position expressed in terms of latitude,
9223 * longitude and height.
9224 * @param oldC previous body-to-NED coordinate transformation.
9225 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
9226 * resolved along NED-frame axes.
9227 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
9228 * resolved along NED-frame axes.
9229 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
9230 * resolved along NED-frame axes.
9231 * @param kinematics body kinematics containing specific forces and angular rates applied to
9232 * the body.
9233 * @return estimated NED frame containing new body position, velocity and coordinate
9234 * transformation matrix.
9235 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
9236 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
9237 * body-to-NED-frame coordinate transformation matrix are
9238 * invalid.
9239 */
9240 public NEDFrame navigateAndReturnNew(
9241 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
9242 final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD, final BodyKinematics kinematics)
9243 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
9244 return navigateAndReturnNew(timeInterval, oldPosition, oldC, oldSpeedN, oldSpeedE, oldSpeedD, kinematics,
9245 DEFAULT_ACCURACY_THRESHOLD);
9246 }
9247
9248 /**
9249 * Runs precision local-navigation-frame inertial navigation equations.
9250 * NOTE: only the attitude update and specific force frame transformation
9251 * phases are precise.
9252 *
9253 * @param timeInterval time interval between epochs expressed in seconds (s).
9254 * @param oldLatitude previous latitude expressed in radians (rad).
9255 * @param oldLongitude previous longitude expressed in radians (rad).
9256 * @param oldHeight previous height expressed in meters (m).
9257 * @param oldC previous body-to-NED coordinate transformation.
9258 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
9259 * resolved along NED-frame axes and expressed in meters per second (m/s).
9260 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
9261 * resolved along NED-frame axes and expressed in meters per second (m/s).
9262 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
9263 * resolved along NED-frame axes and expressed in meters per second (m/s).
9264 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
9265 * resolved along body-frame axes, averaged over time interval.
9266 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
9267 * resolved along body-frame axes, averaged over time interval.
9268 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
9269 * resolved along body-frame axes, averaged over time interval.
9270 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
9271 * resolved along body-frame axes, averaged over time interval and
9272 * expressed in radians per second (rad/s).
9273 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
9274 * resolved along body-frame axes, averaged over time interval and
9275 * expressed in radians per second (rad/s).
9276 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
9277 * resolved along body-frame axes, averaged over time interval and
9278 * expressed in radians per second (rad/s).
9279 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
9280 * @return estimated NED frame containing new body position, velocity and coordinate
9281 * transformation matrix.
9282 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
9283 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
9284 * body-to-NED-frame coordinate transformation matrix are
9285 * invalid.
9286 */
9287 public NEDFrame navigateAndReturnNew(
9288 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
9289 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
9290 final Acceleration fx, final Acceleration fy, final Acceleration fz,
9291 final double angularRateX, final double angularRateY, final double angularRateZ,
9292 final double accuracyThreshold) throws InertialNavigatorException,
9293 InvalidSourceAndDestinationFrameTypeException {
9294 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
9295 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold);
9296 }
9297
9298 /**
9299 * Runs precision local-navigation-frame inertial navigation equations.
9300 * NOTE: only the attitude update and specific force frame transformation
9301 * phases are precise.
9302 *
9303 * @param timeInterval time interval between epochs expressed in seconds (s).
9304 * @param oldLatitude previous latitude expressed in radians (rad).
9305 * @param oldLongitude previous longitude expressed in radians (rad).
9306 * @param oldHeight previous height expressed in meters (m).
9307 * @param oldC previous body-to-NED coordinate transformation.
9308 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
9309 * resolved along NED-frame axes and expressed in meters per second (m/s).
9310 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
9311 * resolved along NED-frame axes and expressed in meters per second (m/s).
9312 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
9313 * resolved along NED-frame axes and expressed in meters per second (m/s).
9314 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
9315 * resolved along body-frame axes, averaged over time interval.
9316 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
9317 * resolved along body-frame axes, averaged over time interval.
9318 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
9319 * resolved along body-frame axes, averaged over time interval.
9320 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
9321 * resolved along body-frame axes, averaged over time interval and
9322 * expressed in radians per second (rad/s).
9323 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
9324 * resolved along body-frame axes, averaged over time interval and
9325 * expressed in radians per second (rad/s).
9326 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
9327 * resolved along body-frame axes, averaged over time interval and
9328 * expressed in radians per second (rad/s).
9329 * @return estimated NED frame containing new body position, velocity and coordinate
9330 * transformation matrix.
9331 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
9332 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
9333 * body-to-NED-frame coordinate transformation matrix are
9334 * invalid.
9335 */
9336 public NEDFrame navigateAndReturnNew(
9337 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
9338 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
9339 final Acceleration fx, final Acceleration fy, final Acceleration fz,
9340 final double angularRateX, final double angularRateY, final double angularRateZ)
9341 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
9342 return navigateAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
9343 fx, fy, fz, angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
9344 }
9345
9346 /**
9347 * Runs precision local-navigation-frame inertial navigation equations.
9348 * NOTE: only the attitude update and specific force frame transformation
9349 * phases are precise.
9350 *
9351 * @param timeInterval time interval between epochs.
9352 * @param oldLatitude previous latitude expressed in radians (rad).
9353 * @param oldLongitude previous longitude expressed in radians (rad).
9354 * @param oldHeight previous height expressed in meters (m).
9355 * @param oldC previous body-to-NED coordinate transformation.
9356 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
9357 * resolved along NED-frame axes and expressed in meters per second (m/s).
9358 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
9359 * resolved along NED-frame axes and expressed in meters per second (m/s).
9360 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
9361 * resolved along NED-frame axes and expressed in meters per second (m/s).
9362 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
9363 * resolved along body-frame axes, averaged over time interval.
9364 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
9365 * resolved along body-frame axes, averaged over time interval.
9366 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
9367 * resolved along body-frame axes, averaged over time interval.
9368 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
9369 * resolved along body-frame axes, averaged over time interval and
9370 * expressed in radians per second (rad/s).
9371 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
9372 * resolved along body-frame axes, averaged over time interval and
9373 * expressed in radians per second (rad/s).
9374 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
9375 * resolved along body-frame axes, averaged over time interval and
9376 * expressed in radians per second (rad/s).
9377 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
9378 * @return estimated NED frame containing new body position, velocity and coordinate
9379 * transformation matrix.
9380 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
9381 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
9382 * body-to-NED-frame coordinate transformation matrix are
9383 * invalid.
9384 */
9385 public NEDFrame navigateAndReturnNew(
9386 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
9387 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
9388 final Acceleration fx, final Acceleration fy, final Acceleration fz,
9389 final double angularRateX, final double angularRateY, final double angularRateZ,
9390 final double accuracyThreshold) throws InertialNavigatorException,
9391 InvalidSourceAndDestinationFrameTypeException {
9392 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
9393 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold);
9394 }
9395
9396 /**
9397 * Runs precision local-navigation-frame inertial navigation equations.
9398 * NOTE: only the attitude update and specific force frame transformation
9399 * phases are precise.
9400 *
9401 * @param timeInterval time interval between epochs.
9402 * @param oldLatitude previous latitude expressed in radians (rad).
9403 * @param oldLongitude previous longitude expressed in radians (rad).
9404 * @param oldHeight previous height expressed in meters (m).
9405 * @param oldC previous body-to-NED coordinate transformation.
9406 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
9407 * resolved along NED-frame axes and expressed in meters per second (m/s).
9408 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
9409 * resolved along NED-frame axes and expressed in meters per second (m/s).
9410 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
9411 * resolved along NED-frame axes and expressed in meters per second (m/s).
9412 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
9413 * resolved along body-frame axes, averaged over time interval.
9414 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
9415 * resolved along body-frame axes, averaged over time interval.
9416 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
9417 * resolved along body-frame axes, averaged over time interval.
9418 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
9419 * resolved along body-frame axes, averaged over time interval and
9420 * expressed in radians per second (rad/s).
9421 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
9422 * resolved along body-frame axes, averaged over time interval and
9423 * expressed in radians per second (rad/s).
9424 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
9425 * resolved along body-frame axes, averaged over time interval and
9426 * expressed in radians per second (rad/s).
9427 * @return estimated NED frame containing new body position, velocity and coordinate
9428 * transformation matrix.
9429 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
9430 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
9431 * body-to-NED-frame coordinate transformation matrix are
9432 * invalid.
9433 */
9434 public NEDFrame navigateAndReturnNew(
9435 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
9436 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
9437 final Acceleration fx, final Acceleration fy, final Acceleration fz,
9438 final double angularRateX, final double angularRateY, final double angularRateZ)
9439 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
9440 return navigateAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
9441 fx, fy, fz, angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
9442 }
9443
9444 /**
9445 * Runs precision local-navigation-frame inertial navigation equations.
9446 * NOTE: only the attitude update and specific force frame transformation
9447 * phases are precise.
9448 *
9449 * @param timeInterval time interval between epochs expressed in seconds (s).
9450 * @param oldPosition previous curvilinear position expressed in terms of latitude,
9451 * longitude and height.
9452 * @param oldC previous body-to-NED coordinate transformation.
9453 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
9454 * resolved along NED-frame axes and expressed in meters per second (m/s).
9455 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
9456 * resolved along NED-frame axes and expressed in meters per second (m/s).
9457 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
9458 * resolved along NED-frame axes and expressed in meters per second (m/s).
9459 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
9460 * resolved along body-frame axes, averaged over time interval.
9461 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
9462 * resolved along body-frame axes, averaged over time interval.
9463 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
9464 * resolved along body-frame axes, averaged over time interval.
9465 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
9466 * resolved along body-frame axes, averaged over time interval and
9467 * expressed in radians per second (rad/s).
9468 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
9469 * resolved along body-frame axes, averaged over time interval and
9470 * expressed in radians per second (rad/s).
9471 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
9472 * resolved along body-frame axes, averaged over time interval and
9473 * expressed in radians per second (rad/s).
9474 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
9475 * @return estimated NED frame containing new body position, velocity and coordinate
9476 * transformation matrix.
9477 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
9478 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
9479 * body-to-NED-frame coordinate transformation matrix are
9480 * invalid.
9481 */
9482 public NEDFrame navigateAndReturnNew(
9483 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
9484 final double oldVn, final double oldVe, final double oldVd,
9485 final Acceleration fx, final Acceleration fy, final Acceleration fz,
9486 final double angularRateX, final double angularRateY, final double angularRateZ,
9487 final double accuracyThreshold) throws InertialNavigatorException,
9488 InvalidSourceAndDestinationFrameTypeException {
9489 return navigateNEDAndReturnNew(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
9490 angularRateX, angularRateY, angularRateZ, accuracyThreshold);
9491 }
9492
9493 /**
9494 * Runs precision local-navigation-frame inertial navigation equations.
9495 * NOTE: only the attitude update and specific force frame transformation
9496 * phases are precise.
9497 *
9498 * @param timeInterval time interval between epochs expressed in seconds (s).
9499 * @param oldPosition previous curvilinear position expressed in terms of latitude,
9500 * longitude and height.
9501 * @param oldC previous body-to-NED coordinate transformation.
9502 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
9503 * resolved along NED-frame axes and expressed in meters per second (m/s).
9504 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
9505 * resolved along NED-frame axes and expressed in meters per second (m/s).
9506 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
9507 * resolved along NED-frame axes and expressed in meters per second (m/s).
9508 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
9509 * resolved along body-frame axes, averaged over time interval.
9510 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
9511 * resolved along body-frame axes, averaged over time interval.
9512 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
9513 * resolved along body-frame axes, averaged over time interval.
9514 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
9515 * resolved along body-frame axes, averaged over time interval and
9516 * expressed in radians per second (rad/s).
9517 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
9518 * resolved along body-frame axes, averaged over time interval and
9519 * expressed in radians per second (rad/s).
9520 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
9521 * resolved along body-frame axes, averaged over time interval and
9522 * expressed in radians per second (rad/s).
9523 * @return estimated NED frame containing new body position, velocity and coordinate
9524 * transformation matrix.
9525 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
9526 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
9527 * body-to-NED-frame coordinate transformation matrix are
9528 * invalid.
9529 */
9530 public NEDFrame navigateAndReturnNew(
9531 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
9532 final double oldVn, final double oldVe, final double oldVd,
9533 final Acceleration fx, final Acceleration fy, final Acceleration fz,
9534 final double angularRateX, final double angularRateY, final double angularRateZ)
9535 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
9536 return navigateAndReturnNew(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
9537 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
9538 }
9539
9540 /**
9541 * Runs precision local-navigation-frame inertial navigation equations.
9542 * NOTE: only the attitude update and specific force frame transformation
9543 * phases are precise.
9544 *
9545 * @param timeInterval time interval between epochs.
9546 * @param oldPosition previous curvilinear position expressed in terms of latitude,
9547 * longitude and height.
9548 * @param oldC previous body-to-NED coordinate transformation.
9549 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
9550 * resolved along NED-frame axes and expressed in meters per second (m/s).
9551 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
9552 * resolved along NED-frame axes and expressed in meters per second (m/s).
9553 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
9554 * resolved along NED-frame axes and expressed in meters per second (m/s).
9555 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
9556 * resolved along body-frame axes, averaged over time interval.
9557 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
9558 * resolved along body-frame axes, averaged over time interval.
9559 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
9560 * resolved along body-frame axes, averaged over time interval.
9561 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
9562 * resolved along body-frame axes, averaged over time interval and
9563 * expressed in radians per second (rad/s).
9564 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
9565 * resolved along body-frame axes, averaged over time interval and
9566 * expressed in radians per second (rad/s).
9567 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
9568 * resolved along body-frame axes, averaged over time interval and
9569 * expressed in radians per second (rad/s).
9570 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
9571 * @return estimated NED frame containing new body position, velocity and coordinate
9572 * transformation matrix.
9573 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
9574 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
9575 * body-to-NED-frame coordinate transformation matrix are
9576 * invalid.
9577 */
9578 public NEDFrame navigateAndReturnNew(
9579 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
9580 final double oldVn, final double oldVe, final double oldVd,
9581 final Acceleration fx, final Acceleration fy, final Acceleration fz,
9582 final double angularRateX, final double angularRateY, final double angularRateZ,
9583 final double accuracyThreshold) throws InertialNavigatorException,
9584 InvalidSourceAndDestinationFrameTypeException {
9585 return navigateNEDAndReturnNew(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
9586 angularRateX, angularRateY, angularRateZ, accuracyThreshold);
9587 }
9588
9589 /**
9590 * Runs precision local-navigation-frame inertial navigation equations.
9591 * NOTE: only the attitude update and specific force frame transformation
9592 * phases are precise.
9593 *
9594 * @param timeInterval time interval between epochs.
9595 * @param oldPosition previous curvilinear position expressed in terms of latitude,
9596 * longitude and height.
9597 * @param oldC previous body-to-NED coordinate transformation.
9598 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
9599 * resolved along NED-frame axes and expressed in meters per second (m/s).
9600 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
9601 * resolved along NED-frame axes and expressed in meters per second (m/s).
9602 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
9603 * resolved along NED-frame axes and expressed in meters per second (m/s).
9604 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
9605 * resolved along body-frame axes, averaged over time interval.
9606 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
9607 * resolved along body-frame axes, averaged over time interval.
9608 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
9609 * resolved along body-frame axes, averaged over time interval.
9610 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
9611 * resolved along body-frame axes, averaged over time interval and
9612 * expressed in radians per second (rad/s).
9613 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
9614 * resolved along body-frame axes, averaged over time interval and
9615 * expressed in radians per second (rad/s).
9616 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
9617 * resolved along body-frame axes, averaged over time interval and
9618 * expressed in radians per second (rad/s).
9619 * @return estimated NED frame containing new body position, velocity and coordinate
9620 * transformation matrix.
9621 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
9622 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
9623 * body-to-NED-frame coordinate transformation matrix are
9624 * invalid.
9625 */
9626 public NEDFrame navigateAndReturnNew(
9627 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
9628 final double oldVn, final double oldVe, final double oldVd,
9629 final Acceleration fx, final Acceleration fy, final Acceleration fz,
9630 final double angularRateX, final double angularRateY, final double angularRateZ)
9631 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
9632 return navigateAndReturnNew(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
9633 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
9634 }
9635
9636 /**
9637 * Runs precision local-navigation-frame inertial navigation equations.
9638 * NOTE: only the attitude update and specific force frame transformation
9639 * phases are precise.
9640 *
9641 * @param timeInterval time interval between epochs expressed in seconds (s).
9642 * @param oldLatitude previous latitude expressed in radians (rad).
9643 * @param oldLongitude previous longitude expressed in radians (rad).
9644 * @param oldHeight previous height expressed in meters (m).
9645 * @param oldC previous body-to-NED coordinate transformation.
9646 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
9647 * along north, east and down axes.
9648 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
9649 * resolved along body-frame axes, averaged over time interval.
9650 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
9651 * resolved along body-frame axes, averaged over time interval.
9652 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
9653 * resolved along body-frame axes, averaged over time interval.
9654 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
9655 * resolved along body-frame axes, averaged over time interval and
9656 * expressed in radians per second (rad/s).
9657 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
9658 * resolved along body-frame axes, averaged over time interval and
9659 * expressed in radians per second (rad/s).
9660 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
9661 * resolved along body-frame axes, averaged over time interval and
9662 * expressed in radians per second (rad/s).
9663 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
9664 * @return estimated NED frame containing new body position, velocity and coordinate
9665 * transformation matrix.
9666 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
9667 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
9668 * body-to-NED-frame coordinate transformation matrix are
9669 * invalid.
9670 */
9671 public NEDFrame navigateAndReturnNew(
9672 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
9673 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
9674 final Acceleration fx, final Acceleration fy, final Acceleration fz,
9675 final double angularRateX, final double angularRateY, final double angularRateZ,
9676 final double accuracyThreshold) throws InertialNavigatorException,
9677 InvalidSourceAndDestinationFrameTypeException {
9678 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity,
9679 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold);
9680 }
9681
9682 /**
9683 * Runs precision local-navigation-frame inertial navigation equations.
9684 * NOTE: only the attitude update and specific force frame transformation
9685 * phases are precise.
9686 *
9687 * @param timeInterval time interval between epochs expressed in seconds (s).
9688 * @param oldLatitude previous latitude expressed in radians (rad).
9689 * @param oldLongitude previous longitude expressed in radians (rad).
9690 * @param oldHeight previous height expressed in meters (m).
9691 * @param oldC previous body-to-NED coordinate transformation.
9692 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
9693 * along north, east and down axes.
9694 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
9695 * resolved along body-frame axes, averaged over time interval.
9696 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
9697 * resolved along body-frame axes, averaged over time interval.
9698 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
9699 * resolved along body-frame axes, averaged over time interval.
9700 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
9701 * resolved along body-frame axes, averaged over time interval and
9702 * expressed in radians per second (rad/s).
9703 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
9704 * resolved along body-frame axes, averaged over time interval and
9705 * expressed in radians per second (rad/s).
9706 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
9707 * resolved along body-frame axes, averaged over time interval and
9708 * expressed in radians per second (rad/s).
9709 * @return estimated NED frame containing new body position, velocity and coordinate
9710 * transformation matrix.
9711 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
9712 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
9713 * body-to-NED-frame coordinate transformation matrix are
9714 * invalid.
9715 */
9716 public NEDFrame navigateAndReturnNew(
9717 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
9718 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
9719 final Acceleration fx, final Acceleration fy, final Acceleration fz,
9720 final double angularRateX, final double angularRateY, final double angularRateZ)
9721 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
9722 return navigateAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
9723 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
9724 }
9725
9726 /**
9727 * Runs precision local-navigation-frame inertial navigation equations.
9728 * NOTE: only the attitude update and specific force frame transformation
9729 * phases are precise.
9730 *
9731 * @param timeInterval time interval between epochs.
9732 * @param oldLatitude previous latitude expressed in radians (rad).
9733 * @param oldLongitude previous longitude expressed in radians (rad).
9734 * @param oldHeight previous height expressed in meters (m).
9735 * @param oldC previous body-to-NED coordinate transformation.
9736 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
9737 * along north, east and down axes.
9738 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
9739 * resolved along body-frame axes, averaged over time interval.
9740 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
9741 * resolved along body-frame axes, averaged over time interval.
9742 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
9743 * resolved along body-frame axes, averaged over time interval.
9744 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
9745 * resolved along body-frame axes, averaged over time interval and
9746 * expressed in radians per second (rad/s).
9747 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
9748 * resolved along body-frame axes, averaged over time interval and
9749 * expressed in radians per second (rad/s).
9750 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
9751 * resolved along body-frame axes, averaged over time interval and
9752 * expressed in radians per second (rad/s).
9753 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
9754 * @return estimated NED frame containing new body position, velocity and coordinate
9755 * transformation matrix.
9756 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
9757 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
9758 * body-to-NED-frame coordinate transformation matrix are
9759 * invalid.
9760 */
9761 public NEDFrame navigateAndReturnNew(
9762 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
9763 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
9764 final Acceleration fx, final Acceleration fy, final Acceleration fz,
9765 final double angularRateX, final double angularRateY, final double angularRateZ,
9766 final double accuracyThreshold) throws InertialNavigatorException,
9767 InvalidSourceAndDestinationFrameTypeException {
9768 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity,
9769 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold);
9770 }
9771
9772 /**
9773 * Runs precision local-navigation-frame inertial navigation equations.
9774 * NOTE: only the attitude update and specific force frame transformation
9775 * phases are precise.
9776 *
9777 * @param timeInterval time interval between epochs.
9778 * @param oldLatitude previous latitude expressed in radians (rad).
9779 * @param oldLongitude previous longitude expressed in radians (rad).
9780 * @param oldHeight previous height expressed in meters (m).
9781 * @param oldC previous body-to-NED coordinate transformation.
9782 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
9783 * along north, east and down axes.
9784 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
9785 * resolved along body-frame axes, averaged over time interval.
9786 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
9787 * resolved along body-frame axes, averaged over time interval.
9788 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
9789 * resolved along body-frame axes, averaged over time interval.
9790 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
9791 * resolved along body-frame axes, averaged over time interval and
9792 * expressed in radians per second (rad/s).
9793 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
9794 * resolved along body-frame axes, averaged over time interval and
9795 * expressed in radians per second (rad/s).
9796 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
9797 * resolved along body-frame axes, averaged over time interval and
9798 * expressed in radians per second (rad/s).
9799 * @return estimated NED frame containing new body position, velocity and coordinate
9800 * transformation matrix.
9801 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
9802 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
9803 * body-to-NED-frame coordinate transformation matrix are
9804 * invalid.
9805 */
9806 public NEDFrame navigateAndReturnNew(
9807 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
9808 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
9809 final Acceleration fx, final Acceleration fy, final Acceleration fz,
9810 final double angularRateX, final double angularRateY, final double angularRateZ)
9811 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
9812 return navigateAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
9813 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
9814 }
9815
9816 /**
9817 * Runs precision local-navigation-frame inertial navigation equations.
9818 * NOTE: only the attitude update and specific force frame transformation
9819 * phases are precise.
9820 *
9821 * @param timeInterval time interval between epochs expressed in seconds (s).
9822 * @param oldPosition previous curvilinear position expressed in terms of latitude,
9823 * longitude and height.
9824 * @param oldC previous body-to-NED coordinate transformation.
9825 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
9826 * along north, east and down axes.
9827 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
9828 * resolved along body-frame axes, averaged over time interval.
9829 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
9830 * resolved along body-frame axes, averaged over time interval.
9831 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
9832 * resolved along body-frame axes, averaged over time interval.
9833 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
9834 * resolved along body-frame axes, averaged over time interval and
9835 * expressed in radians per second (rad/s).
9836 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
9837 * resolved along body-frame axes, averaged over time interval and
9838 * expressed in radians per second (rad/s).
9839 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
9840 * resolved along body-frame axes, averaged over time interval and
9841 * expressed in radians per second (rad/s).
9842 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
9843 * @return estimated NED frame containing new body position, velocity and coordinate
9844 * transformation matrix.
9845 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
9846 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
9847 * body-to-NED-frame coordinate transformation matrix are
9848 * invalid.
9849 */
9850 public NEDFrame navigateAndReturnNew(
9851 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
9852 final NEDVelocity oldVelocity, final Acceleration fx, final Acceleration fy, final Acceleration fz,
9853 final double angularRateX, final double angularRateY, final double angularRateZ,
9854 final double accuracyThreshold) throws InertialNavigatorException,
9855 InvalidSourceAndDestinationFrameTypeException {
9856 return navigateNEDAndReturnNew(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz,
9857 angularRateX, angularRateY, angularRateZ, accuracyThreshold);
9858 }
9859
9860 /**
9861 * Runs precision local-navigation-frame inertial navigation equations.
9862 * NOTE: only the attitude update and specific force frame transformation
9863 * phases are precise.
9864 *
9865 * @param timeInterval time interval between epochs expressed in seconds (s).
9866 * @param oldPosition previous curvilinear position expressed in terms of latitude,
9867 * longitude and height.
9868 * @param oldC previous body-to-NED coordinate transformation.
9869 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
9870 * along north, east and down axes.
9871 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
9872 * resolved along body-frame axes, averaged over time interval.
9873 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
9874 * resolved along body-frame axes, averaged over time interval.
9875 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
9876 * resolved along body-frame axes, averaged over time interval.
9877 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
9878 * resolved along body-frame axes, averaged over time interval and
9879 * expressed in radians per second (rad/s).
9880 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
9881 * resolved along body-frame axes, averaged over time interval and
9882 * expressed in radians per second (rad/s).
9883 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
9884 * resolved along body-frame axes, averaged over time interval and
9885 * expressed in radians per second (rad/s).
9886 * @return estimated NED frame containing new body position, velocity and coordinate
9887 * transformation matrix.
9888 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
9889 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
9890 * body-to-NED-frame coordinate transformation matrix are
9891 * invalid.
9892 */
9893 public NEDFrame navigateAndReturnNew(
9894 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
9895 final NEDVelocity oldVelocity, final Acceleration fx, final Acceleration fy, final Acceleration fz,
9896 final double angularRateX, final double angularRateY, final double angularRateZ)
9897 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
9898 return navigateAndReturnNew(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz,
9899 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
9900 }
9901
9902 /**
9903 * Runs precision local-navigation-frame inertial navigation equations.
9904 * NOTE: only the attitude update and specific force frame transformation
9905 * phases are precise.
9906 *
9907 * @param timeInterval time interval between epochs.
9908 * @param oldPosition previous curvilinear position expressed in terms of latitude,
9909 * longitude and height.
9910 * @param oldC previous body-to-NED coordinate transformation.
9911 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
9912 * along north, east and down axes.
9913 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
9914 * resolved along body-frame axes, averaged over time interval.
9915 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
9916 * resolved along body-frame axes, averaged over time interval.
9917 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
9918 * resolved along body-frame axes, averaged over time interval.
9919 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
9920 * resolved along body-frame axes, averaged over time interval and
9921 * expressed in radians per second (rad/s).
9922 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
9923 * resolved along body-frame axes, averaged over time interval and
9924 * expressed in radians per second (rad/s).
9925 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
9926 * resolved along body-frame axes, averaged over time interval and
9927 * expressed in radians per second (rad/s).
9928 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
9929 * @return estimated NED frame containing new body position, velocity and coordinate
9930 * transformation matrix.
9931 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
9932 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
9933 * body-to-NED-frame coordinate transformation matrix are
9934 * invalid.
9935 */
9936 public NEDFrame navigateAndReturnNew(
9937 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
9938 final NEDVelocity oldVelocity, final Acceleration fx, final Acceleration fy, final Acceleration fz,
9939 final double angularRateX, final double angularRateY, final double angularRateZ,
9940 final double accuracyThreshold) throws InertialNavigatorException,
9941 InvalidSourceAndDestinationFrameTypeException {
9942 return navigateNEDAndReturnNew(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz,
9943 angularRateX, angularRateY, angularRateZ, accuracyThreshold);
9944 }
9945
9946 /**
9947 * Runs precision local-navigation-frame inertial navigation equations.
9948 * NOTE: only the attitude update and specific force frame transformation
9949 * phases are precise.
9950 *
9951 * @param timeInterval time interval between epochs.
9952 * @param oldPosition previous curvilinear position expressed in terms of latitude,
9953 * longitude and height.
9954 * @param oldC previous body-to-NED coordinate transformation.
9955 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
9956 * along north, east and down axes.
9957 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
9958 * resolved along body-frame axes, averaged over time interval.
9959 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
9960 * resolved along body-frame axes, averaged over time interval.
9961 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
9962 * resolved along body-frame axes, averaged over time interval.
9963 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
9964 * resolved along body-frame axes, averaged over time interval and
9965 * expressed in radians per second (rad/s).
9966 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
9967 * resolved along body-frame axes, averaged over time interval and
9968 * expressed in radians per second (rad/s).
9969 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
9970 * resolved along body-frame axes, averaged over time interval and
9971 * expressed in radians per second (rad/s).
9972 * @return estimated NED frame containing new body position, velocity and coordinate
9973 * transformation matrix.
9974 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
9975 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
9976 * body-to-NED-frame coordinate transformation matrix are
9977 * invalid.
9978 */
9979 public NEDFrame navigateAndReturnNew(
9980 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
9981 final NEDVelocity oldVelocity, final Acceleration fx, final Acceleration fy, final Acceleration fz,
9982 final double angularRateX, final double angularRateY, final double angularRateZ)
9983 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
9984 return navigateAndReturnNew(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz,
9985 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
9986 }
9987
9988 /**
9989 * Runs precision local-navigation-frame inertial navigation equations.
9990 * NOTE: only the attitude update and specific force frame transformation
9991 * phases are precise.
9992 *
9993 * @param timeInterval time interval between epochs expressed in seconds (s).
9994 * @param oldLatitude previous latitude expressed in radians (rad).
9995 * @param oldLongitude previous longitude expressed in radians (rad).
9996 * @param oldHeight previous height expressed in meters (m).
9997 * @param oldC previous body-to-NED coordinate transformation.
9998 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
9999 * resolved along NED-frame axes and expressed in meters per second (m/s).
10000 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
10001 * resolved along NED-frame axes and expressed in meters per second (m/s).
10002 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
10003 * resolved along NED-frame axes and expressed in meters per second (m/s).
10004 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
10005 * resolved along body-frame axes, averaged over time interval and
10006 * expressed in meters per squared second (m/s^2).
10007 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
10008 * resolved along body-frame axes, averaged over time interval and
10009 * expressed in meters per squared second (m/s^2).
10010 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
10011 * resolved along body-frame axes, averaged over time interval and
10012 * expressed in meters per squared second (m/s^2).
10013 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
10014 * resolved along body-frame axes, averaged over time interval.
10015 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
10016 * resolved along body-frame axes, averaged over time interval.
10017 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
10018 * resolved along body-frame axes, averaged over time interval.
10019 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
10020 * @return estimated NED frame containing new body position, velocity and coordinate
10021 * transformation matrix.
10022 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
10023 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
10024 * body-to-NED-frame coordinate transformation matrix are
10025 * invalid.
10026 */
10027 public NEDFrame navigateAndReturnNew(
10028 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
10029 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
10030 final double fx, final double fy, final double fz,
10031 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
10032 final double accuracyThreshold) throws InertialNavigatorException,
10033 InvalidSourceAndDestinationFrameTypeException {
10034 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
10035 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold);
10036 }
10037
10038 /**
10039 * Runs precision local-navigation-frame inertial navigation equations.
10040 * NOTE: only the attitude update and specific force frame transformation
10041 * phases are precise.
10042 *
10043 * @param timeInterval time interval between epochs expressed in seconds (s).
10044 * @param oldLatitude previous latitude expressed in radians (rad).
10045 * @param oldLongitude previous longitude expressed in radians (rad).
10046 * @param oldHeight previous height expressed in meters (m).
10047 * @param oldC previous body-to-NED coordinate transformation.
10048 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
10049 * resolved along NED-frame axes and expressed in meters per second (m/s).
10050 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
10051 * resolved along NED-frame axes and expressed in meters per second (m/s).
10052 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
10053 * resolved along NED-frame axes and expressed in meters per second (m/s).
10054 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
10055 * resolved along body-frame axes, averaged over time interval and
10056 * expressed in meters per squared second (m/s^2).
10057 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
10058 * resolved along body-frame axes, averaged over time interval and
10059 * expressed in meters per squared second (m/s^2).
10060 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
10061 * resolved along body-frame axes, averaged over time interval and
10062 * expressed in meters per squared second (m/s^2).
10063 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
10064 * resolved along body-frame axes, averaged over time interval.
10065 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
10066 * resolved along body-frame axes, averaged over time interval.
10067 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
10068 * resolved along body-frame axes, averaged over time interval.
10069 * @return estimated NED frame containing new body position, velocity and coordinate
10070 * transformation matrix.
10071 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
10072 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
10073 * body-to-NED-frame coordinate transformation matrix are
10074 * invalid.
10075 */
10076 public NEDFrame navigateAndReturnNew(
10077 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
10078 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
10079 final double fx, final double fy, final double fz,
10080 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
10081 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
10082 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
10083 fx, fy, fz, angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
10084 }
10085
10086 /**
10087 * Runs precision local-navigation-frame inertial navigation equations.
10088 * NOTE: only the attitude update and specific force frame transformation
10089 * phases are precise.
10090 *
10091 * @param timeInterval time interval between epochs.
10092 * @param oldLatitude previous latitude expressed in radians (rad).
10093 * @param oldLongitude previous longitude expressed in radians (rad).
10094 * @param oldHeight previous height expressed in meters (m).
10095 * @param oldC previous body-to-NED coordinate transformation.
10096 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
10097 * resolved along NED-frame axes and expressed in meters per second (m/s).
10098 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
10099 * resolved along NED-frame axes and expressed in meters per second (m/s).
10100 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
10101 * resolved along NED-frame axes and expressed in meters per second (m/s).
10102 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
10103 * resolved along body-frame axes, averaged over time interval and
10104 * expressed in meters per squared second (m/s^2).
10105 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
10106 * resolved along body-frame axes, averaged over time interval and
10107 * expressed in meters per squared second (m/s^2).
10108 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
10109 * resolved along body-frame axes, averaged over time interval and
10110 * expressed in meters per squared second (m/s^2).
10111 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
10112 * resolved along body-frame axes, averaged over time interval.
10113 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
10114 * resolved along body-frame axes, averaged over time interval.
10115 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
10116 * resolved along body-frame axes, averaged over time interval.
10117 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
10118 * @return estimated NED frame containing new body position, velocity and coordinate
10119 * transformation matrix.
10120 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
10121 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
10122 * body-to-NED-frame coordinate transformation matrix are
10123 * invalid.
10124 */
10125 public NEDFrame navigateAndReturnNew(
10126 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
10127 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
10128 final double fx, final double fy, final double fz,
10129 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
10130 final double accuracyThreshold) throws InertialNavigatorException,
10131 InvalidSourceAndDestinationFrameTypeException {
10132 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
10133 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold);
10134 }
10135
10136 /**
10137 * Runs precision local-navigation-frame inertial navigation equations.
10138 * NOTE: only the attitude update and specific force frame transformation
10139 * phases are precise.
10140 *
10141 * @param timeInterval time interval between epochs.
10142 * @param oldLatitude previous latitude expressed in radians (rad).
10143 * @param oldLongitude previous longitude expressed in radians (rad).
10144 * @param oldHeight previous height expressed in meters (m).
10145 * @param oldC previous body-to-NED coordinate transformation.
10146 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
10147 * resolved along NED-frame axes and expressed in meters per second (m/s).
10148 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
10149 * resolved along NED-frame axes and expressed in meters per second (m/s).
10150 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
10151 * resolved along NED-frame axes and expressed in meters per second (m/s).
10152 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
10153 * resolved along body-frame axes, averaged over time interval and
10154 * expressed in meters per squared second (m/s^2).
10155 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
10156 * resolved along body-frame axes, averaged over time interval and
10157 * expressed in meters per squared second (m/s^2).
10158 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
10159 * resolved along body-frame axes, averaged over time interval and
10160 * expressed in meters per squared second (m/s^2).
10161 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
10162 * resolved along body-frame axes, averaged over time interval.
10163 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
10164 * resolved along body-frame axes, averaged over time interval.
10165 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
10166 * resolved along body-frame axes, averaged over time interval.
10167 * @return estimated NED frame containing new body position, velocity and coordinate
10168 * transformation matrix.
10169 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
10170 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
10171 * body-to-NED-frame coordinate transformation matrix are
10172 * invalid.
10173 */
10174 public NEDFrame navigateAndReturnNew(
10175 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
10176 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
10177 final double fx, final double fy, final double fz,
10178 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
10179 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
10180 return navigateAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
10181 fx, fy, fz, angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
10182 }
10183
10184 /**
10185 * Runs precision local-navigation-frame inertial navigation equations.
10186 * NOTE: only the attitude update and specific force frame transformation
10187 * phases are precise.
10188 *
10189 * @param timeInterval time interval between epochs expressed in seconds (s).
10190 * @param oldPosition previous curvilinear position expressed in terms of latitude,
10191 * longitude and height.
10192 * @param oldC previous body-to-NED coordinate transformation.
10193 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
10194 * resolved along NED-frame axes and expressed in meters per second (m/s).
10195 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
10196 * resolved along NED-frame axes and expressed in meters per second (m/s).
10197 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
10198 * resolved along NED-frame axes and expressed in meters per second (m/s).
10199 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
10200 * resolved along body-frame axes, averaged over time interval and
10201 * expressed in meters per squared second (m/s^2).
10202 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
10203 * resolved along body-frame axes, averaged over time interval and
10204 * expressed in meters per squared second (m/s^2).
10205 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
10206 * resolved along body-frame axes, averaged over time interval and
10207 * expressed in meters per squared second (m/s^2).
10208 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
10209 * resolved along body-frame axes, averaged over time interval.
10210 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
10211 * resolved along body-frame axes, averaged over time interval.
10212 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
10213 * resolved along body-frame axes, averaged over time interval.
10214 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
10215 * @return estimated NED frame containing new body position, velocity and coordinate
10216 * transformation matrix.
10217 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
10218 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
10219 * body-to-NED-frame coordinate transformation matrix are
10220 * invalid.
10221 */
10222 public NEDFrame navigateAndReturnNew(
10223 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
10224 final double oldVn, final double oldVe, final double oldVd,
10225 final double fx, final double fy, final double fz,
10226 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
10227 final double accuracyThreshold) throws InertialNavigatorException,
10228 InvalidSourceAndDestinationFrameTypeException {
10229 return navigateNEDAndReturnNew(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
10230 angularRateX, angularRateY, angularRateZ, accuracyThreshold);
10231 }
10232
10233 /**
10234 * Runs precision local-navigation-frame inertial navigation equations.
10235 * NOTE: only the attitude update and specific force frame transformation
10236 * phases are precise.
10237 *
10238 * @param timeInterval time interval between epochs expressed in seconds (s).
10239 * @param oldPosition previous curvilinear position expressed in terms of latitude,
10240 * longitude and height.
10241 * @param oldC previous body-to-NED coordinate transformation.
10242 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
10243 * resolved along NED-frame axes and expressed in meters per second (m/s).
10244 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
10245 * resolved along NED-frame axes and expressed in meters per second (m/s).
10246 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
10247 * resolved along NED-frame axes and expressed in meters per second (m/s).
10248 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
10249 * resolved along body-frame axes, averaged over time interval and
10250 * expressed in meters per squared second (m/s^2).
10251 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
10252 * resolved along body-frame axes, averaged over time interval and
10253 * expressed in meters per squared second (m/s^2).
10254 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
10255 * resolved along body-frame axes, averaged over time interval and
10256 * expressed in meters per squared second (m/s^2).
10257 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
10258 * resolved along body-frame axes, averaged over time interval.
10259 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
10260 * resolved along body-frame axes, averaged over time interval.
10261 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
10262 * resolved along body-frame axes, averaged over time interval.
10263 * @return estimated NED frame containing new body position, velocity and coordinate
10264 * transformation matrix.
10265 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
10266 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
10267 * body-to-NED-frame coordinate transformation matrix are
10268 * invalid.
10269 */
10270 public NEDFrame navigateAndReturnNew(
10271 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
10272 final double oldVn, final double oldVe, final double oldVd,
10273 final double fx, final double fy, final double fz,
10274 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
10275 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
10276 return navigateAndReturnNew(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
10277 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
10278 }
10279
10280 /**
10281 * Runs precision local-navigation-frame inertial navigation equations.
10282 * NOTE: only the attitude update and specific force frame transformation
10283 * phases are precise.
10284 *
10285 * @param timeInterval time interval between epochs.
10286 * @param oldPosition previous curvilinear position expressed in terms of latitude,
10287 * longitude and height.
10288 * @param oldC previous body-to-NED coordinate transformation.
10289 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
10290 * resolved along NED-frame axes and expressed in meters per second (m/s).
10291 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
10292 * resolved along NED-frame axes and expressed in meters per second (m/s).
10293 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
10294 * resolved along NED-frame axes and expressed in meters per second (m/s).
10295 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
10296 * resolved along body-frame axes, averaged over time interval and
10297 * expressed in meters per squared second (m/s^2).
10298 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
10299 * resolved along body-frame axes, averaged over time interval and
10300 * expressed in meters per squared second (m/s^2).
10301 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
10302 * resolved along body-frame axes, averaged over time interval and
10303 * expressed in meters per squared second (m/s^2).
10304 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
10305 * resolved along body-frame axes, averaged over time interval.
10306 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
10307 * resolved along body-frame axes, averaged over time interval.
10308 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
10309 * resolved along body-frame axes, averaged over time interval.
10310 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
10311 * @return estimated NED frame containing new body position, velocity and coordinate
10312 * transformation matrix.
10313 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
10314 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
10315 * body-to-NED-frame coordinate transformation matrix are
10316 * invalid.
10317 */
10318 public NEDFrame navigateAndReturnNew(
10319 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
10320 final double oldVn, final double oldVe, final double oldVd,
10321 final double fx, final double fy, final double fz,
10322 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
10323 final double accuracyThreshold) throws InertialNavigatorException,
10324 InvalidSourceAndDestinationFrameTypeException {
10325 return navigateNEDAndReturnNew(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
10326 angularRateX, angularRateY, angularRateZ, accuracyThreshold);
10327 }
10328
10329 /**
10330 * Runs precision local-navigation-frame inertial navigation equations.
10331 * NOTE: only the attitude update and specific force frame transformation
10332 * phases are precise.
10333 *
10334 * @param timeInterval time interval between epochs.
10335 * @param oldPosition previous curvilinear position expressed in terms of latitude,
10336 * longitude and height.
10337 * @param oldC previous body-to-NED coordinate transformation.
10338 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
10339 * resolved along NED-frame axes and expressed in meters per second (m/s).
10340 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
10341 * resolved along NED-frame axes and expressed in meters per second (m/s).
10342 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
10343 * resolved along NED-frame axes and expressed in meters per second (m/s).
10344 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
10345 * resolved along body-frame axes, averaged over time interval and
10346 * expressed in meters per squared second (m/s^2).
10347 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
10348 * resolved along body-frame axes, averaged over time interval and
10349 * expressed in meters per squared second (m/s^2).
10350 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
10351 * resolved along body-frame axes, averaged over time interval and
10352 * expressed in meters per squared second (m/s^2).
10353 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
10354 * resolved along body-frame axes, averaged over time interval.
10355 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
10356 * resolved along body-frame axes, averaged over time interval.
10357 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
10358 * resolved along body-frame axes, averaged over time interval.
10359 * @return estimated NED frame containing new body position, velocity and coordinate
10360 * transformation matrix.
10361 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
10362 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
10363 * body-to-NED-frame coordinate transformation matrix are
10364 * invalid.
10365 */
10366 public NEDFrame navigateAndReturnNew(
10367 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
10368 final double oldVn, final double oldVe, final double oldVd,
10369 final double fx, final double fy, final double fz,
10370 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
10371 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
10372 return navigateNEDAndReturnNew(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
10373 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
10374 }
10375
10376 /**
10377 * Runs precision local-navigation-frame inertial navigation equations.
10378 * NOTE: only the attitude update and specific force frame transformation
10379 * phases are precise.
10380 *
10381 * @param timeInterval time interval between epochs expressed in seconds (s).
10382 * @param oldLatitude previous latitude expressed in radians (rad).
10383 * @param oldLongitude previous longitude expressed in radians (rad).
10384 * @param oldHeight previous height expressed in meters (m).
10385 * @param oldC previous body-to-NED coordinate transformation.
10386 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
10387 * along north, east and down axes.
10388 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
10389 * resolved along body-frame axes, averaged over time interval and
10390 * expressed in meters per squared second (m/s^2).
10391 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
10392 * resolved along body-frame axes, averaged over time interval and
10393 * expressed in meters per squared second (m/s^2).
10394 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
10395 * resolved along body-frame axes, averaged over time interval and
10396 * expressed in meters per squared second (m/s^2).
10397 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
10398 * resolved along body-frame axes, averaged over time interval.
10399 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
10400 * resolved along body-frame axes, averaged over time interval.
10401 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
10402 * resolved along body-frame axes, averaged over time interval.
10403 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
10404 * @return estimated NED frame containing new body position, velocity and coordinate
10405 * transformation matrix.
10406 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
10407 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
10408 * body-to-NED-frame coordinate transformation matrix are
10409 * invalid.
10410 */
10411 public NEDFrame navigateAndReturnNew(
10412 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
10413 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
10414 final double fx, final double fy, final double fz,
10415 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
10416 final double accuracyThreshold) throws InertialNavigatorException,
10417 InvalidSourceAndDestinationFrameTypeException {
10418 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity,
10419 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold);
10420 }
10421
10422 /**
10423 * Runs precision local-navigation-frame inertial navigation equations.
10424 * NOTE: only the attitude update and specific force frame transformation
10425 * phases are precise.
10426 *
10427 * @param timeInterval time interval between epochs expressed in seconds (s).
10428 * @param oldLatitude previous latitude expressed in radians (rad).
10429 * @param oldLongitude previous longitude expressed in radians (rad).
10430 * @param oldHeight previous height expressed in meters (m).
10431 * @param oldC previous body-to-NED coordinate transformation.
10432 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
10433 * along north, east and down axes.
10434 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
10435 * resolved along body-frame axes, averaged over time interval and
10436 * expressed in meters per squared second (m/s^2).
10437 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
10438 * resolved along body-frame axes, averaged over time interval and
10439 * expressed in meters per squared second (m/s^2).
10440 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
10441 * resolved along body-frame axes, averaged over time interval and
10442 * expressed in meters per squared second (m/s^2).
10443 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
10444 * resolved along body-frame axes, averaged over time interval.
10445 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
10446 * resolved along body-frame axes, averaged over time interval.
10447 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
10448 * resolved along body-frame axes, averaged over time interval.
10449 * @return estimated NED frame containing new body position, velocity and coordinate
10450 * transformation matrix.
10451 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
10452 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
10453 * body-to-NED-frame coordinate transformation matrix are
10454 * invalid.
10455 */
10456 public NEDFrame navigateAndReturnNew(
10457 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
10458 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
10459 final double fx, final double fy, final double fz,
10460 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
10461 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
10462 return navigateAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
10463 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
10464 }
10465
10466 /**
10467 * Runs precision local-navigation-frame inertial navigation equations.
10468 * NOTE: only the attitude update and specific force frame transformation
10469 * phases are precise.
10470 *
10471 * @param timeInterval time interval between epochs.
10472 * @param oldLatitude previous latitude expressed in radians (rad).
10473 * @param oldLongitude previous longitude expressed in radians (rad).
10474 * @param oldHeight previous height expressed in meters (m).
10475 * @param oldC previous body-to-NED coordinate transformation.
10476 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
10477 * along north, east and down axes.
10478 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
10479 * resolved along body-frame axes, averaged over time interval and
10480 * expressed in meters per squared second (m/s^2).
10481 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
10482 * resolved along body-frame axes, averaged over time interval and
10483 * expressed in meters per squared second (m/s^2).
10484 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
10485 * resolved along body-frame axes, averaged over time interval and
10486 * expressed in meters per squared second (m/s^2).
10487 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
10488 * resolved along body-frame axes, averaged over time interval.
10489 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
10490 * resolved along body-frame axes, averaged over time interval.
10491 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
10492 * resolved along body-frame axes, averaged over time interval.
10493 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
10494 * @return estimated NED frame containing new body position, velocity and coordinate
10495 * transformation matrix.
10496 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
10497 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
10498 * body-to-NED-frame coordinate transformation matrix are
10499 * invalid.
10500 */
10501 public NEDFrame navigateAndReturnNew(
10502 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
10503 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
10504 final double fx, final double fy, final double fz,
10505 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
10506 final double accuracyThreshold) throws InertialNavigatorException,
10507 InvalidSourceAndDestinationFrameTypeException {
10508 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity,
10509 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold);
10510 }
10511
10512 /**
10513 * Runs precision local-navigation-frame inertial navigation equations.
10514 * NOTE: only the attitude update and specific force frame transformation
10515 * phases are precise.
10516 *
10517 * @param timeInterval time interval between epochs.
10518 * @param oldLatitude previous latitude expressed in radians (rad).
10519 * @param oldLongitude previous longitude expressed in radians (rad).
10520 * @param oldHeight previous height expressed in meters (m).
10521 * @param oldC previous body-to-NED coordinate transformation.
10522 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
10523 * along north, east and down axes.
10524 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
10525 * resolved along body-frame axes, averaged over time interval and
10526 * expressed in meters per squared second (m/s^2).
10527 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
10528 * resolved along body-frame axes, averaged over time interval and
10529 * expressed in meters per squared second (m/s^2).
10530 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
10531 * resolved along body-frame axes, averaged over time interval and
10532 * expressed in meters per squared second (m/s^2).
10533 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
10534 * resolved along body-frame axes, averaged over time interval.
10535 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
10536 * resolved along body-frame axes, averaged over time interval.
10537 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
10538 * resolved along body-frame axes, averaged over time interval.
10539 * @return estimated NED frame containing new body position, velocity and coordinate
10540 * transformation matrix.
10541 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
10542 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
10543 * body-to-NED-frame coordinate transformation matrix are
10544 * invalid.
10545 */
10546 public NEDFrame navigateAndReturnNew(
10547 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
10548 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
10549 final double fx, final double fy, final double fz,
10550 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
10551 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
10552 return navigateAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity,
10553 fx, fy, fz, angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
10554 }
10555
10556 /**
10557 * Runs precision local-navigation-frame inertial navigation equations.
10558 * NOTE: only the attitude update and specific force frame transformation
10559 * phases are precise.
10560 *
10561 * @param timeInterval time interval between epochs expressed in seconds (s).
10562 * @param oldPosition previous curvilinear position expressed in terms of latitude,
10563 * longitude and height.
10564 * @param oldC previous body-to-NED coordinate transformation.
10565 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
10566 * along north, east and down axes.
10567 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
10568 * resolved along body-frame axes, averaged over time interval and
10569 * expressed in meters per squared second (m/s^2).
10570 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
10571 * resolved along body-frame axes, averaged over time interval and
10572 * expressed in meters per squared second (m/s^2).
10573 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
10574 * resolved along body-frame axes, averaged over time interval and
10575 * expressed in meters per squared second (m/s^2).
10576 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
10577 * resolved along body-frame axes, averaged over time interval.
10578 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
10579 * resolved along body-frame axes, averaged over time interval.
10580 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
10581 * resolved along body-frame axes, averaged over time interval.
10582 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
10583 * @return estimated NED frame containing new body position, velocity and coordinate
10584 * transformation matrix.
10585 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
10586 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
10587 * body-to-NED-frame coordinate transformation matrix are
10588 * invalid.
10589 */
10590 public NEDFrame navigateAndReturnNew(
10591 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
10592 final NEDVelocity oldVelocity, final double fx, final double fy, final double fz,
10593 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
10594 final double accuracyThreshold) throws InertialNavigatorException,
10595 InvalidSourceAndDestinationFrameTypeException {
10596 return navigateNEDAndReturnNew(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz,
10597 angularRateX, angularRateY, angularRateZ, accuracyThreshold);
10598 }
10599
10600 /**
10601 * Runs precision local-navigation-frame inertial navigation equations.
10602 * NOTE: only the attitude update and specific force frame transformation
10603 * phases are precise.
10604 *
10605 * @param timeInterval time interval between epochs expressed in seconds (s).
10606 * @param oldPosition previous curvilinear position expressed in terms of latitude,
10607 * longitude and height.
10608 * @param oldC previous body-to-NED coordinate transformation.
10609 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
10610 * along north, east and down axes.
10611 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
10612 * resolved along body-frame axes, averaged over time interval and
10613 * expressed in meters per squared second (m/s^2).
10614 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
10615 * resolved along body-frame axes, averaged over time interval and
10616 * expressed in meters per squared second (m/s^2).
10617 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
10618 * resolved along body-frame axes, averaged over time interval and
10619 * expressed in meters per squared second (m/s^2).
10620 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
10621 * resolved along body-frame axes, averaged over time interval.
10622 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
10623 * resolved along body-frame axes, averaged over time interval.
10624 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
10625 * resolved along body-frame axes, averaged over time interval.
10626 * @return estimated NED frame containing new body position, velocity and coordinate
10627 * transformation matrix.
10628 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
10629 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
10630 * body-to-NED-frame coordinate transformation matrix are
10631 * invalid.
10632 */
10633 public NEDFrame navigateAndReturnNew(
10634 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
10635 final NEDVelocity oldVelocity, final double fx, final double fy, final double fz,
10636 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
10637 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
10638 return navigateAndReturnNew(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz,
10639 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
10640 }
10641
10642 /**
10643 * Runs precision local-navigation-frame inertial navigation equations.
10644 * NOTE: only the attitude update and specific force frame transformation
10645 * phases are precise.
10646 *
10647 * @param timeInterval time interval between epochs.
10648 * @param oldPosition previous curvilinear position expressed in terms of latitude,
10649 * longitude and height.
10650 * @param oldC previous body-to-NED coordinate transformation.
10651 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
10652 * along north, east and down axes.
10653 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
10654 * resolved along body-frame axes, averaged over time interval and
10655 * expressed in meters per squared second (m/s^2).
10656 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
10657 * resolved along body-frame axes, averaged over time interval and
10658 * expressed in meters per squared second (m/s^2).
10659 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
10660 * resolved along body-frame axes, averaged over time interval and
10661 * expressed in meters per squared second (m/s^2).
10662 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
10663 * resolved along body-frame axes, averaged over time interval.
10664 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
10665 * resolved along body-frame axes, averaged over time interval.
10666 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
10667 * resolved along body-frame axes, averaged over time interval.
10668 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
10669 * @return estimated NED frame containing new body position, velocity and coordinate
10670 * transformation matrix.
10671 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
10672 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
10673 * body-to-NED-frame coordinate transformation matrix are
10674 * invalid.
10675 */
10676 public NEDFrame navigateAndReturnNew(
10677 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
10678 final NEDVelocity oldVelocity, final double fx, final double fy, final double fz,
10679 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
10680 final double accuracyThreshold) throws InertialNavigatorException,
10681 InvalidSourceAndDestinationFrameTypeException {
10682 return navigateNEDAndReturnNew(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz,
10683 angularRateX, angularRateY, angularRateZ, accuracyThreshold);
10684 }
10685
10686 /**
10687 * Runs precision local-navigation-frame inertial navigation equations.
10688 * NOTE: only the attitude update and specific force frame transformation
10689 * phases are precise.
10690 *
10691 * @param timeInterval time interval between epochs.
10692 * @param oldPosition previous curvilinear position expressed in terms of latitude,
10693 * longitude and height.
10694 * @param oldC previous body-to-NED coordinate transformation.
10695 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
10696 * along north, east and down axes.
10697 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
10698 * resolved along body-frame axes, averaged over time interval and
10699 * expressed in meters per squared second (m/s^2).
10700 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
10701 * resolved along body-frame axes, averaged over time interval and
10702 * expressed in meters per squared second (m/s^2).
10703 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
10704 * resolved along body-frame axes, averaged over time interval and
10705 * expressed in meters per squared second (m/s^2).
10706 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
10707 * resolved along body-frame axes, averaged over time interval.
10708 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
10709 * resolved along body-frame axes, averaged over time interval.
10710 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
10711 * resolved along body-frame axes, averaged over time interval.
10712 * @return estimated NED frame containing new body position, velocity and coordinate
10713 * transformation matrix.
10714 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
10715 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
10716 * body-to-NED-frame coordinate transformation matrix are
10717 * invalid.
10718 */
10719 public NEDFrame navigateAndReturnNew(
10720 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
10721 final NEDVelocity oldVelocity, final double fx, final double fy, final double fz,
10722 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
10723 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
10724 return navigateAndReturnNew(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz,
10725 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
10726 }
10727
10728 /**
10729 * Runs precision local-navigation-frame inertial navigation equations.
10730 * NOTE: only the attitude update and specific force frame transformation
10731 * phases are precise.
10732 *
10733 * @param timeInterval time interval between epochs expressed in seconds (s).
10734 * @param oldLatitude previous latitude angle.
10735 * @param oldLongitude previous longitude angle.
10736 * @param oldHeight previous height.
10737 * @param oldC previous body-to-NED coordinate transformation.
10738 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
10739 * resolved along NED-frame axes.
10740 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
10741 * resolved along NED-frame axes.
10742 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
10743 * resolved along NED-frame axes.
10744 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
10745 * resolved along body-frame axes, averaged over time interval and
10746 * expressed in meters per squared second (m/s^2).
10747 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
10748 * resolved along body-frame axes, averaged over time interval and
10749 * expressed in meters per squared second (m/s^2).
10750 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
10751 * resolved along body-frame axes, averaged over time interval and
10752 * expressed in meters per squared second (m/s^2).
10753 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
10754 * resolved along body-frame axes, averaged over time interval and
10755 * expressed in radians per second (rad/s).
10756 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
10757 * resolved along body-frame axes, averaged over time interval and
10758 * expressed in radians per second (rad/s).
10759 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
10760 * resolved along body-frame axes, averaged over time interval and
10761 * expressed in radians per second (rad/s).
10762 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
10763 * @return estimated NED frame containing new body position, velocity and coordinate
10764 * transformation matrix.
10765 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
10766 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
10767 * body-to-NED-frame coordinate transformation matrix are
10768 * invalid.
10769 */
10770 public NEDFrame navigateAndReturnNew(
10771 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
10772 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
10773 final double fx, final double fy, final double fz,
10774 final double angularRateX, final double angularRateY, final double angularRateZ,
10775 final double accuracyThreshold) throws InertialNavigatorException,
10776 InvalidSourceAndDestinationFrameTypeException {
10777 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
10778 oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
10779 accuracyThreshold);
10780 }
10781
10782 /**
10783 * Runs precision local-navigation-frame inertial navigation equations.
10784 * NOTE: only the attitude update and specific force frame transformation
10785 * phases are precise.
10786 *
10787 * @param timeInterval time interval between epochs expressed in seconds (s).
10788 * @param oldLatitude previous latitude angle.
10789 * @param oldLongitude previous longitude angle.
10790 * @param oldHeight previous height.
10791 * @param oldC previous body-to-NED coordinate transformation.
10792 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
10793 * resolved along NED-frame axes.
10794 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
10795 * resolved along NED-frame axes.
10796 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
10797 * resolved along NED-frame axes.
10798 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
10799 * resolved along body-frame axes, averaged over time interval and
10800 * expressed in meters per squared second (m/s^2).
10801 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
10802 * resolved along body-frame axes, averaged over time interval and
10803 * expressed in meters per squared second (m/s^2).
10804 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
10805 * resolved along body-frame axes, averaged over time interval and
10806 * expressed in meters per squared second (m/s^2).
10807 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
10808 * resolved along body-frame axes, averaged over time interval and
10809 * expressed in radians per second (rad/s).
10810 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
10811 * resolved along body-frame axes, averaged over time interval and
10812 * expressed in radians per second (rad/s).
10813 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
10814 * resolved along body-frame axes, averaged over time interval and
10815 * expressed in radians per second (rad/s).
10816 * @return estimated NED frame containing new body position, velocity and coordinate
10817 * transformation matrix.
10818 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
10819 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
10820 * body-to-NED-frame coordinate transformation matrix are
10821 * invalid.
10822 */
10823 public NEDFrame navigateAndReturnNew(
10824 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
10825 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
10826 final double fx, final double fy, final double fz,
10827 final double angularRateX, final double angularRateY, final double angularRateZ)
10828 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
10829 return navigateAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
10830 oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
10831 DEFAULT_ACCURACY_THRESHOLD);
10832 }
10833
10834 /**
10835 * Runs precision local-navigation-frame inertial navigation equations.
10836 * NOTE: only the attitude update and specific force frame transformation
10837 * phases are precise.
10838 *
10839 * @param timeInterval time interval between epochs.
10840 * @param oldLatitude previous latitude angle.
10841 * @param oldLongitude previous longitude angle.
10842 * @param oldHeight previous height.
10843 * @param oldC previous body-to-NED coordinate transformation.
10844 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
10845 * resolved along NED-frame axes.
10846 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
10847 * resolved along NED-frame axes.
10848 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
10849 * resolved along NED-frame axes.
10850 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
10851 * resolved along body-frame axes, averaged over time interval and
10852 * expressed in meters per squared second (m/s^2).
10853 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
10854 * resolved along body-frame axes, averaged over time interval and
10855 * expressed in meters per squared second (m/s^2).
10856 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
10857 * resolved along body-frame axes, averaged over time interval and
10858 * expressed in meters per squared second (m/s^2).
10859 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
10860 * resolved along body-frame axes, averaged over time interval and
10861 * expressed in radians per second (rad/s).
10862 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
10863 * resolved along body-frame axes, averaged over time interval and
10864 * expressed in radians per second (rad/s).
10865 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
10866 * resolved along body-frame axes, averaged over time interval and
10867 * expressed in radians per second (rad/s).
10868 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
10869 * @return estimated NED frame containing new body position, velocity and coordinate
10870 * transformation matrix.
10871 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
10872 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
10873 * body-to-NED-frame coordinate transformation matrix are
10874 * invalid.
10875 */
10876 public NEDFrame navigateAndReturnNew(
10877 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
10878 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
10879 final double fx, final double fy, final double fz,
10880 final double angularRateX, final double angularRateY, final double angularRateZ,
10881 final double accuracyThreshold) throws InertialNavigatorException,
10882 InvalidSourceAndDestinationFrameTypeException {
10883 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
10884 oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
10885 accuracyThreshold);
10886 }
10887
10888 /**
10889 * Runs precision local-navigation-frame inertial navigation equations.
10890 * NOTE: only the attitude update and specific force frame transformation
10891 * phases are precise.
10892 *
10893 * @param timeInterval time interval between epochs.
10894 * @param oldLatitude previous latitude angle.
10895 * @param oldLongitude previous longitude angle.
10896 * @param oldHeight previous height.
10897 * @param oldC previous body-to-NED coordinate transformation.
10898 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
10899 * resolved along NED-frame axes.
10900 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
10901 * resolved along NED-frame axes.
10902 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
10903 * resolved along NED-frame axes.
10904 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
10905 * resolved along body-frame axes, averaged over time interval and
10906 * expressed in meters per squared second (m/s^2).
10907 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
10908 * resolved along body-frame axes, averaged over time interval and
10909 * expressed in meters per squared second (m/s^2).
10910 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
10911 * resolved along body-frame axes, averaged over time interval and
10912 * expressed in meters per squared second (m/s^2).
10913 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
10914 * resolved along body-frame axes, averaged over time interval and
10915 * expressed in radians per second (rad/s).
10916 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
10917 * resolved along body-frame axes, averaged over time interval and
10918 * expressed in radians per second (rad/s).
10919 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
10920 * resolved along body-frame axes, averaged over time interval and
10921 * expressed in radians per second (rad/s).
10922 * @return estimated NED frame containing new body position, velocity and coordinate
10923 * transformation matrix.
10924 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
10925 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
10926 * body-to-NED-frame coordinate transformation matrix are
10927 * invalid.
10928 */
10929 public NEDFrame navigateAndReturnNew(
10930 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
10931 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
10932 final double fx, final double fy, final double fz,
10933 final double angularRateX, final double angularRateY, final double angularRateZ)
10934 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
10935 return navigateAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
10936 oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
10937 DEFAULT_ACCURACY_THRESHOLD);
10938 }
10939
10940 /**
10941 * Runs precision local-navigation-frame inertial navigation equations.
10942 * NOTE: only the attitude update and specific force frame transformation
10943 * phases are precise.
10944 *
10945 * @param timeInterval time interval between epochs expressed in seconds (s).
10946 * @param oldLatitude previous latitude angle.
10947 * @param oldLongitude previous longitude angle.
10948 * @param oldHeight previous height.
10949 * @param oldC previous body-to-NED coordinate transformation.
10950 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
10951 * resolved along NED-frame axes.
10952 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
10953 * resolved along NED-frame axes.
10954 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
10955 * resolved along NED-frame axes.
10956 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
10957 * resolved along body-frame axes, averaged over time interval.
10958 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
10959 * resolved along body-frame axes, averaged over time interval.
10960 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
10961 * resolved along body-frame axes, averaged over time interval.
10962 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
10963 * resolved along body-frame axes, averaged over time interval.
10964 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
10965 * resolved along body-frame axes, averaged over time interval.
10966 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
10967 * resolved along body-frame axes, averaged over time interval.
10968 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
10969 * @return estimated NED frame containing new body position, velocity and coordinate
10970 * transformation matrix.
10971 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
10972 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
10973 * body-to-NED-frame coordinate transformation matrix are
10974 * invalid.
10975 */
10976 public NEDFrame navigateAndReturnNew(
10977 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
10978 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
10979 final Acceleration fx, final Acceleration fy, final Acceleration fz,
10980 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
10981 final double accuracyThreshold) throws InertialNavigatorException,
10982 InvalidSourceAndDestinationFrameTypeException {
10983 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
10984 oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
10985 accuracyThreshold);
10986 }
10987
10988 /**
10989 * Runs precision local-navigation-frame inertial navigation equations.
10990 * NOTE: only the attitude update and specific force frame transformation
10991 * phases are precise.
10992 *
10993 * @param timeInterval time interval between epochs expressed in seconds (s).
10994 * @param oldLatitude previous latitude angle.
10995 * @param oldLongitude previous longitude angle.
10996 * @param oldHeight previous height.
10997 * @param oldC previous body-to-NED coordinate transformation.
10998 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
10999 * resolved along NED-frame axes.
11000 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
11001 * resolved along NED-frame axes.
11002 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
11003 * resolved along NED-frame axes.
11004 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
11005 * resolved along body-frame axes, averaged over time interval.
11006 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
11007 * resolved along body-frame axes, averaged over time interval.
11008 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
11009 * resolved along body-frame axes, averaged over time interval.
11010 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
11011 * resolved along body-frame axes, averaged over time interval.
11012 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
11013 * resolved along body-frame axes, averaged over time interval.
11014 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
11015 * resolved along body-frame axes, averaged over time interval.
11016 * @return estimated NED frame containing new body position, velocity and coordinate
11017 * transformation matrix.
11018 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
11019 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
11020 * body-to-NED-frame coordinate transformation matrix are
11021 * invalid.
11022 */
11023 public NEDFrame navigateAndReturnNew(
11024 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
11025 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
11026 final Acceleration fx, final Acceleration fy, final Acceleration fz,
11027 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
11028 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
11029 return navigateAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
11030 oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
11031 DEFAULT_ACCURACY_THRESHOLD);
11032 }
11033
11034 /**
11035 * Runs precision local-navigation-frame inertial navigation equations.
11036 * NOTE: only the attitude update and specific force frame transformation
11037 * phases are precise.
11038 *
11039 * @param timeInterval time interval between epochs.
11040 * @param oldLatitude previous latitude angle.
11041 * @param oldLongitude previous longitude angle.
11042 * @param oldHeight previous height.
11043 * @param oldC previous body-to-NED coordinate transformation.
11044 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
11045 * resolved along NED-frame axes.
11046 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
11047 * resolved along NED-frame axes.
11048 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
11049 * resolved along NED-frame axes.
11050 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
11051 * resolved along body-frame axes, averaged over time interval.
11052 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
11053 * resolved along body-frame axes, averaged over time interval.
11054 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
11055 * resolved along body-frame axes, averaged over time interval.
11056 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
11057 * resolved along body-frame axes, averaged over time interval.
11058 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
11059 * resolved along body-frame axes, averaged over time interval.
11060 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
11061 * resolved along body-frame axes, averaged over time interval.
11062 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
11063 * @return estimated NED frame containing new body position, velocity and coordinate
11064 * transformation matrix.
11065 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
11066 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
11067 * body-to-NED-frame coordinate transformation matrix are
11068 * invalid.
11069 */
11070 public NEDFrame navigateAndReturnNew(
11071 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
11072 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
11073 final Acceleration fx, final Acceleration fy, final Acceleration fz,
11074 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
11075 final double accuracyThreshold) throws InertialNavigatorException,
11076 InvalidSourceAndDestinationFrameTypeException {
11077 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
11078 oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
11079 accuracyThreshold);
11080 }
11081
11082 /**
11083 * Runs precision local-navigation-frame inertial navigation equations.
11084 * NOTE: only the attitude update and specific force frame transformation
11085 * phases are precise.
11086 *
11087 * @param timeInterval time interval between epochs.
11088 * @param oldLatitude previous latitude angle.
11089 * @param oldLongitude previous longitude angle.
11090 * @param oldHeight previous height.
11091 * @param oldC previous body-to-NED coordinate transformation.
11092 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
11093 * resolved along NED-frame axes.
11094 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
11095 * resolved along NED-frame axes.
11096 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
11097 * resolved along NED-frame axes.
11098 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
11099 * resolved along body-frame axes, averaged over time interval.
11100 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
11101 * resolved along body-frame axes, averaged over time interval.
11102 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
11103 * resolved along body-frame axes, averaged over time interval.
11104 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
11105 * resolved along body-frame axes, averaged over time interval.
11106 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
11107 * resolved along body-frame axes, averaged over time interval.
11108 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
11109 * resolved along body-frame axes, averaged over time interval.
11110 * @return estimated NED frame containing new body position, velocity and coordinate
11111 * transformation matrix.
11112 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
11113 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
11114 * body-to-NED-frame coordinate transformation matrix are
11115 * invalid.
11116 */
11117 public NEDFrame navigateAndReturnNew(
11118 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
11119 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
11120 final Acceleration fx, final Acceleration fy, final Acceleration fz,
11121 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
11122 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
11123 return navigateAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
11124 oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
11125 DEFAULT_ACCURACY_THRESHOLD);
11126 }
11127
11128 /**
11129 * Runs precision local-navigation-frame inertial navigation equations.
11130 * NOTE: only the attitude update and specific force frame transformation
11131 * phases are precise.
11132 *
11133 * @param timeInterval time interval between epochs expressed in seconds (s).
11134 * @param oldLatitude previous latitude expressed in radians (rad).
11135 * @param oldLongitude previous longitude expressed in radians (rad).
11136 * @param oldHeight previous height expressed in meters (m).
11137 * @param oldC previous body-to-NED coordinate transformation.
11138 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
11139 * resolved along NED-frame axes.
11140 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
11141 * resolved along NED-frame axes.
11142 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
11143 * resolved along NED-frame axes.
11144 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
11145 * resolved along body-frame axes, averaged over time interval.
11146 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
11147 * resolved along body-frame axes, averaged over time interval.
11148 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
11149 * resolved along body-frame axes, averaged over time interval.
11150 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
11151 * resolved along body-frame axes, averaged over time interval.
11152 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
11153 * resolved along body-frame axes, averaged over time interval.
11154 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
11155 * resolved along body-frame axes, averaged over time interval.
11156 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
11157 * @return estimated NED frame containing new body position, velocity and coordinate
11158 * transformation matrix.
11159 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
11160 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
11161 * body-to-NED-frame coordinate transformation matrix are
11162 * invalid.
11163 */
11164 public NEDFrame navigateAndReturnNew(
11165 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
11166 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
11167 final Acceleration fx, final Acceleration fy, final Acceleration fz,
11168 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
11169 final double accuracyThreshold) throws InertialNavigatorException,
11170 InvalidSourceAndDestinationFrameTypeException {
11171 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
11172 oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
11173 accuracyThreshold);
11174 }
11175
11176 /**
11177 * Runs precision local-navigation-frame inertial navigation equations.
11178 * NOTE: only the attitude update and specific force frame transformation
11179 * phases are precise.
11180 *
11181 * @param timeInterval time interval between epochs expressed in seconds (s).
11182 * @param oldLatitude previous latitude expressed in radians (rad).
11183 * @param oldLongitude previous longitude expressed in radians (rad).
11184 * @param oldHeight previous height expressed in meters (m).
11185 * @param oldC previous body-to-NED coordinate transformation.
11186 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
11187 * resolved along NED-frame axes.
11188 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
11189 * resolved along NED-frame axes.
11190 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
11191 * resolved along NED-frame axes.
11192 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
11193 * resolved along body-frame axes, averaged over time interval.
11194 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
11195 * resolved along body-frame axes, averaged over time interval.
11196 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
11197 * resolved along body-frame axes, averaged over time interval.
11198 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
11199 * resolved along body-frame axes, averaged over time interval.
11200 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
11201 * resolved along body-frame axes, averaged over time interval.
11202 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
11203 * resolved along body-frame axes, averaged over time interval.
11204 * @return estimated NED frame containing new body position, velocity and coordinate
11205 * transformation matrix.
11206 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
11207 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
11208 * body-to-NED-frame coordinate transformation matrix are
11209 * invalid.
11210 */
11211 public NEDFrame navigateAndReturnNew(
11212 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
11213 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
11214 final Acceleration fx, final Acceleration fy, final Acceleration fz,
11215 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
11216 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
11217 return navigateAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
11218 oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
11219 DEFAULT_ACCURACY_THRESHOLD);
11220 }
11221
11222 /**
11223 * Runs precision local-navigation-frame inertial navigation equations.
11224 * NOTE: only the attitude update and specific force frame transformation
11225 * phases are precise.
11226 *
11227 * @param timeInterval time interval between epochs.
11228 * @param oldLatitude previous latitude expressed in radians (rad).
11229 * @param oldLongitude previous longitude expressed in radians (rad).
11230 * @param oldHeight previous height expressed in meters (m).
11231 * @param oldC previous body-to-NED coordinate transformation.
11232 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
11233 * resolved along NED-frame axes.
11234 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
11235 * resolved along NED-frame axes.
11236 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
11237 * resolved along NED-frame axes.
11238 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
11239 * resolved along body-frame axes, averaged over time interval.
11240 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
11241 * resolved along body-frame axes, averaged over time interval.
11242 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
11243 * resolved along body-frame axes, averaged over time interval.
11244 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
11245 * resolved along body-frame axes, averaged over time interval.
11246 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
11247 * resolved along body-frame axes, averaged over time interval.
11248 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
11249 * resolved along body-frame axes, averaged over time interval.
11250 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
11251 * @return estimated NED frame containing new body position, velocity and coordinate
11252 * transformation matrix.
11253 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
11254 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
11255 * body-to-NED-frame coordinate transformation matrix are
11256 * invalid.
11257 */
11258 public NEDFrame navigateAndReturnNew(
11259 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
11260 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
11261 final Acceleration fx, final Acceleration fy, final Acceleration fz,
11262 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
11263 final double accuracyThreshold) throws InertialNavigatorException,
11264 InvalidSourceAndDestinationFrameTypeException {
11265 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
11266 oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
11267 accuracyThreshold);
11268 }
11269
11270 /**
11271 * Runs precision local-navigation-frame inertial navigation equations.
11272 * NOTE: only the attitude update and specific force frame transformation
11273 * phases are precise.
11274 *
11275 * @param timeInterval time interval between epochs.
11276 * @param oldLatitude previous latitude expressed in radians (rad).
11277 * @param oldLongitude previous longitude expressed in radians (rad).
11278 * @param oldHeight previous height expressed in meters (m).
11279 * @param oldC previous body-to-NED coordinate transformation.
11280 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
11281 * resolved along NED-frame axes.
11282 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
11283 * resolved along NED-frame axes.
11284 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
11285 * resolved along NED-frame axes.
11286 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
11287 * resolved along body-frame axes, averaged over time interval.
11288 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
11289 * resolved along body-frame axes, averaged over time interval.
11290 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
11291 * resolved along body-frame axes, averaged over time interval.
11292 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
11293 * resolved along body-frame axes, averaged over time interval.
11294 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
11295 * resolved along body-frame axes, averaged over time interval.
11296 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
11297 * resolved along body-frame axes, averaged over time interval.
11298 * @return estimated NED frame containing new body position, velocity and coordinate
11299 * transformation matrix.
11300 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
11301 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
11302 * body-to-NED-frame coordinate transformation matrix are
11303 * invalid.
11304 */
11305 public NEDFrame navigateAndReturnNew(
11306 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
11307 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
11308 final Acceleration fx, final Acceleration fy, final Acceleration fz,
11309 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
11310 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
11311 return navigateAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
11312 oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
11313 DEFAULT_ACCURACY_THRESHOLD);
11314 }
11315
11316 /**
11317 * Runs precision local-navigation-frame inertial navigation equations.
11318 * NOTE: only the attitude update and specific force frame transformation
11319 * phases are precise.
11320 *
11321 * @param timeInterval time interval between epochs expressed in seconds (s).
11322 * @param oldPosition previous curvilinear position expressed in terms of latitude,
11323 * longitude and height.
11324 * @param oldC previous body-to-NED coordinate transformation.
11325 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
11326 * resolved along NED-frame axes.
11327 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
11328 * resolved along NED-frame axes.
11329 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
11330 * resolved along NED-frame axes.
11331 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
11332 * resolved along body-frame axes, averaged over time interval.
11333 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
11334 * resolved along body-frame axes, averaged over time interval.
11335 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
11336 * resolved along body-frame axes, averaged over time interval.
11337 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
11338 * resolved along body-frame axes, averaged over time interval.
11339 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
11340 * resolved along body-frame axes, averaged over time interval.
11341 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
11342 * resolved along body-frame axes, averaged over time interval.
11343 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
11344 * @return estimated NED frame containing new body position, velocity and coordinate
11345 * transformation matrix.
11346 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
11347 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
11348 * body-to-NED-frame coordinate transformation matrix are
11349 * invalid.
11350 */
11351 public NEDFrame navigateAndReturnNew(
11352 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
11353 final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
11354 final Acceleration fx, final Acceleration fy, final Acceleration fz,
11355 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
11356 final double accuracyThreshold) throws InertialNavigatorException,
11357 InvalidSourceAndDestinationFrameTypeException {
11358 return navigateNEDAndReturnNew(timeInterval, oldPosition, oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
11359 angularRateX, angularRateY, angularRateZ, accuracyThreshold);
11360 }
11361
11362 /**
11363 * Runs precision local-navigation-frame inertial navigation equations.
11364 * NOTE: only the attitude update and specific force frame transformation
11365 * phases are precise.
11366 *
11367 * @param timeInterval time interval between epochs expressed in seconds (s).
11368 * @param oldPosition previous curvilinear position expressed in terms of latitude,
11369 * longitude and height.
11370 * @param oldC previous body-to-NED coordinate transformation.
11371 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
11372 * resolved along NED-frame axes.
11373 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
11374 * resolved along NED-frame axes.
11375 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
11376 * resolved along NED-frame axes.
11377 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
11378 * resolved along body-frame axes, averaged over time interval.
11379 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
11380 * resolved along body-frame axes, averaged over time interval.
11381 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
11382 * resolved along body-frame axes, averaged over time interval.
11383 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
11384 * resolved along body-frame axes, averaged over time interval.
11385 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
11386 * resolved along body-frame axes, averaged over time interval.
11387 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
11388 * resolved along body-frame axes, averaged over time interval.
11389 * @return estimated NED frame containing new body position, velocity and coordinate
11390 * transformation matrix.
11391 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
11392 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
11393 * body-to-NED-frame coordinate transformation matrix are
11394 * invalid.
11395 */
11396 public NEDFrame navigateAndReturnNew(
11397 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
11398 final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
11399 final Acceleration fx, final Acceleration fy, final Acceleration fz,
11400 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
11401 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
11402 return navigateAndReturnNew(timeInterval, oldPosition, oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
11403 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
11404 }
11405
11406 /**
11407 * Runs precision local-navigation-frame inertial navigation equations.
11408 * NOTE: only the attitude update and specific force frame transformation
11409 * phases are precise.
11410 *
11411 * @param timeInterval time interval between epochs.
11412 * @param oldPosition previous curvilinear position expressed in terms of latitude,
11413 * longitude and height.
11414 * @param oldC previous body-to-NED coordinate transformation.
11415 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
11416 * resolved along NED-frame axes.
11417 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
11418 * resolved along NED-frame axes.
11419 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
11420 * resolved along NED-frame axes.
11421 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
11422 * resolved along body-frame axes, averaged over time interval.
11423 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
11424 * resolved along body-frame axes, averaged over time interval.
11425 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
11426 * resolved along body-frame axes, averaged over time interval.
11427 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
11428 * resolved along body-frame axes, averaged over time interval.
11429 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
11430 * resolved along body-frame axes, averaged over time interval.
11431 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
11432 * resolved along body-frame axes, averaged over time interval.
11433 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
11434 * @return estimated NED frame containing new body position, velocity and coordinate
11435 * transformation matrix.
11436 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
11437 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
11438 * body-to-NED-frame coordinate transformation matrix are
11439 * invalid.
11440 */
11441 public NEDFrame navigateAndReturnNew(
11442 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
11443 final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
11444 final Acceleration fx, final Acceleration fy, final Acceleration fz,
11445 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
11446 final double accuracyThreshold) throws InertialNavigatorException,
11447 InvalidSourceAndDestinationFrameTypeException {
11448 return navigateNEDAndReturnNew(timeInterval, oldPosition, oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
11449 angularRateX, angularRateY, angularRateZ, accuracyThreshold);
11450 }
11451
11452 /**
11453 * Runs precision local-navigation-frame inertial navigation equations.
11454 * NOTE: only the attitude update and specific force frame transformation
11455 * phases are precise.
11456 *
11457 * @param timeInterval time interval between epochs.
11458 * @param oldPosition previous curvilinear position expressed in terms of latitude,
11459 * longitude and height.
11460 * @param oldC previous body-to-NED coordinate transformation.
11461 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
11462 * resolved along NED-frame axes.
11463 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
11464 * resolved along NED-frame axes.
11465 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
11466 * resolved along NED-frame axes.
11467 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
11468 * resolved along body-frame axes, averaged over time interval.
11469 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
11470 * resolved along body-frame axes, averaged over time interval.
11471 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
11472 * resolved along body-frame axes, averaged over time interval.
11473 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
11474 * resolved along body-frame axes, averaged over time interval.
11475 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
11476 * resolved along body-frame axes, averaged over time interval.
11477 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
11478 * resolved along body-frame axes, averaged over time interval.
11479 * @return estimated NED frame containing new body position, velocity and coordinate
11480 * transformation matrix.
11481 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
11482 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
11483 * body-to-NED-frame coordinate transformation matrix are
11484 * invalid.
11485 */
11486 public NEDFrame navigateAndReturnNew(
11487 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
11488 final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
11489 final Acceleration fx, final Acceleration fy, final Acceleration fz,
11490 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
11491 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
11492 return navigateAndReturnNew(timeInterval, oldPosition, oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
11493 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
11494 }
11495
11496 /**
11497 * Runs precision local-navigation-frame inertial navigation equations.
11498 * NOTE: only the attitude update and specific force frame transformation
11499 * phases are precise.
11500 *
11501 * @param timeInterval time interval between epochs expressed in seconds (s).
11502 * @param oldLatitude previous latitude angle.
11503 * @param oldLongitude previous longitude angle.
11504 * @param oldHeight previous height.
11505 * @param oldC previous body-to-NED coordinate transformation.
11506 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
11507 * along north, east and down axes.
11508 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
11509 * resolved along body-frame axes, averaged over time interval.
11510 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
11511 * resolved along body-frame axes, averaged over time interval.
11512 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
11513 * resolved along body-frame axes, averaged over time interval.
11514 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
11515 * resolved along body-frame axes, averaged over time interval.
11516 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
11517 * resolved along body-frame axes, averaged over time interval.
11518 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
11519 * resolved along body-frame axes, averaged over time interval.
11520 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
11521 * @return estimated NED frame containing new body position, velocity and coordinate
11522 * transformation matrix.
11523 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
11524 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
11525 * body-to-NED-frame coordinate transformation matrix are
11526 * invalid.
11527 */
11528 public NEDFrame navigateAndReturnNew(
11529 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
11530 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
11531 final Acceleration fx, final Acceleration fy, final Acceleration fz,
11532 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
11533 final double accuracyThreshold) throws InertialNavigatorException,
11534 InvalidSourceAndDestinationFrameTypeException {
11535 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity,
11536 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold);
11537 }
11538
11539 /**
11540 * Runs precision local-navigation-frame inertial navigation equations.
11541 * NOTE: only the attitude update and specific force frame transformation
11542 * phases are precise.
11543 *
11544 * @param timeInterval time interval between epochs expressed in seconds (s).
11545 * @param oldLatitude previous latitude angle.
11546 * @param oldLongitude previous longitude angle.
11547 * @param oldHeight previous height.
11548 * @param oldC previous body-to-NED coordinate transformation.
11549 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
11550 * along north, east and down axes.
11551 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
11552 * resolved along body-frame axes, averaged over time interval.
11553 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
11554 * resolved along body-frame axes, averaged over time interval.
11555 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
11556 * resolved along body-frame axes, averaged over time interval.
11557 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
11558 * resolved along body-frame axes, averaged over time interval.
11559 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
11560 * resolved along body-frame axes, averaged over time interval.
11561 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
11562 * resolved along body-frame axes, averaged over time interval.
11563 * @return estimated NED frame containing new body position, velocity and coordinate
11564 * transformation matrix.
11565 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
11566 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
11567 * body-to-NED-frame coordinate transformation matrix are
11568 * invalid.
11569 */
11570 public NEDFrame navigateAndReturnNew(
11571 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
11572 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
11573 final Acceleration fx, final Acceleration fy, final Acceleration fz,
11574 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
11575 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
11576 return navigateAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
11577 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
11578 }
11579
11580 /**
11581 * Runs precision local-navigation-frame inertial navigation equations.
11582 * NOTE: only the attitude update and specific force frame transformation
11583 * phases are precise.
11584 *
11585 * @param timeInterval time interval between epochs.
11586 * @param oldLatitude previous latitude angle.
11587 * @param oldLongitude previous longitude angle.
11588 * @param oldHeight previous height.
11589 * @param oldC previous body-to-NED coordinate transformation.
11590 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
11591 * along north, east and down axes.
11592 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
11593 * resolved along body-frame axes, averaged over time interval.
11594 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
11595 * resolved along body-frame axes, averaged over time interval.
11596 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
11597 * resolved along body-frame axes, averaged over time interval.
11598 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
11599 * resolved along body-frame axes, averaged over time interval.
11600 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
11601 * resolved along body-frame axes, averaged over time interval.
11602 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
11603 * resolved along body-frame axes, averaged over time interval.
11604 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
11605 * @return estimated NED frame containing new body position, velocity and coordinate
11606 * transformation matrix.
11607 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
11608 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
11609 * body-to-NED-frame coordinate transformation matrix are
11610 * invalid.
11611 */
11612 public NEDFrame navigateAndReturnNew(
11613 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
11614 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
11615 final Acceleration fx, final Acceleration fy, final Acceleration fz,
11616 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
11617 final double accuracyThreshold) throws InertialNavigatorException,
11618 InvalidSourceAndDestinationFrameTypeException {
11619 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity,
11620 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold);
11621 }
11622
11623 /**
11624 * Runs precision local-navigation-frame inertial navigation equations.
11625 * NOTE: only the attitude update and specific force frame transformation
11626 * phases are precise.
11627 *
11628 * @param timeInterval time interval between epochs.
11629 * @param oldLatitude previous latitude angle.
11630 * @param oldLongitude previous longitude angle.
11631 * @param oldHeight previous height.
11632 * @param oldC previous body-to-NED coordinate transformation.
11633 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
11634 * along north, east and down axes.
11635 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
11636 * resolved along body-frame axes, averaged over time interval.
11637 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
11638 * resolved along body-frame axes, averaged over time interval.
11639 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
11640 * resolved along body-frame axes, averaged over time interval.
11641 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
11642 * resolved along body-frame axes, averaged over time interval.
11643 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
11644 * resolved along body-frame axes, averaged over time interval.
11645 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
11646 * resolved along body-frame axes, averaged over time interval.
11647 * @return estimated NED frame containing new body position, velocity and coordinate
11648 * transformation matrix.
11649 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
11650 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
11651 * body-to-NED-frame coordinate transformation matrix are
11652 * invalid.
11653 */
11654 public NEDFrame navigateAndReturnNew(
11655 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
11656 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
11657 final Acceleration fx, final Acceleration fy, final Acceleration fz,
11658 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
11659 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
11660 return navigateAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
11661 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
11662 }
11663
11664 /**
11665 * Runs precision local-navigation-frame inertial navigation equations.
11666 * NOTE: only the attitude update and specific force frame transformation
11667 * phases are precise.
11668 *
11669 * @param timeInterval time interval between epochs expressed in seconds (s).
11670 * @param oldPosition previous curvilinear position expressed in terms of latitude,
11671 * longitude and height.
11672 * @param oldC previous body-to-NED coordinate transformation.
11673 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
11674 * along north, east and down axes.
11675 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
11676 * resolved along body-frame axes, averaged over time interval.
11677 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
11678 * resolved along body-frame axes, averaged over time interval.
11679 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
11680 * resolved along body-frame axes, averaged over time interval.
11681 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
11682 * resolved along body-frame axes, averaged over time interval.
11683 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
11684 * resolved along body-frame axes, averaged over time interval.
11685 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
11686 * resolved along body-frame axes, averaged over time interval.
11687 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
11688 * @return estimated NED frame containing new body position, velocity and coordinate
11689 * transformation matrix.
11690 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
11691 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
11692 * body-to-NED-frame coordinate transformation matrix are
11693 * invalid.
11694 */
11695 public NEDFrame navigateAndReturnNew(
11696 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
11697 final NEDVelocity oldVelocity, final Acceleration fx, final Acceleration fy, final Acceleration fz,
11698 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
11699 final double accuracyThreshold) throws InertialNavigatorException,
11700 InvalidSourceAndDestinationFrameTypeException {
11701 return navigateNEDAndReturnNew(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz,
11702 angularRateX, angularRateY, angularRateZ, accuracyThreshold);
11703 }
11704
11705 /**
11706 * Runs precision local-navigation-frame inertial navigation equations.
11707 * NOTE: only the attitude update and specific force frame transformation
11708 * phases are precise.
11709 *
11710 * @param timeInterval time interval between epochs expressed in seconds (s).
11711 * @param oldPosition previous curvilinear position expressed in terms of latitude,
11712 * longitude and height.
11713 * @param oldC previous body-to-NED coordinate transformation.
11714 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
11715 * along north, east and down axes.
11716 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
11717 * resolved along body-frame axes, averaged over time interval.
11718 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
11719 * resolved along body-frame axes, averaged over time interval.
11720 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
11721 * resolved along body-frame axes, averaged over time interval.
11722 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
11723 * resolved along body-frame axes, averaged over time interval.
11724 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
11725 * resolved along body-frame axes, averaged over time interval.
11726 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
11727 * resolved along body-frame axes, averaged over time interval.
11728 * @return estimated NED frame containing new body position, velocity and coordinate
11729 * transformation matrix.
11730 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
11731 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
11732 * body-to-NED-frame coordinate transformation matrix are
11733 * invalid.
11734 */
11735 public NEDFrame navigateAndReturnNew(
11736 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
11737 final NEDVelocity oldVelocity, final Acceleration fx, final Acceleration fy, final Acceleration fz,
11738 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
11739 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
11740 return navigateAndReturnNew(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz,
11741 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
11742 }
11743
11744 /**
11745 * Runs precision local-navigation-frame inertial navigation equations.
11746 * NOTE: only the attitude update and specific force frame transformation
11747 * phases are precise.
11748 *
11749 * @param timeInterval time interval between epochs.
11750 * @param oldPosition previous curvilinear position expressed in terms of latitude,
11751 * longitude and height.
11752 * @param oldC previous body-to-NED coordinate transformation.
11753 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
11754 * along north, east and down axes.
11755 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
11756 * resolved along body-frame axes, averaged over time interval.
11757 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
11758 * resolved along body-frame axes, averaged over time interval.
11759 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
11760 * resolved along body-frame axes, averaged over time interval.
11761 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
11762 * resolved along body-frame axes, averaged over time interval.
11763 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
11764 * resolved along body-frame axes, averaged over time interval.
11765 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
11766 * resolved along body-frame axes, averaged over time interval.
11767 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
11768 * @return estimated NED frame containing new body position, velocity and coordinate
11769 * transformation matrix.
11770 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
11771 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
11772 * body-to-NED-frame coordinate transformation matrix are
11773 * invalid.
11774 */
11775 public NEDFrame navigateAndReturnNew(
11776 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
11777 final NEDVelocity oldVelocity, final Acceleration fx, final Acceleration fy, final Acceleration fz,
11778 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
11779 final double accuracyThreshold) throws InertialNavigatorException,
11780 InvalidSourceAndDestinationFrameTypeException {
11781 return navigateNEDAndReturnNew(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz,
11782 angularRateX, angularRateY, angularRateZ, accuracyThreshold);
11783 }
11784
11785 /**
11786 * Runs precision local-navigation-frame inertial navigation equations.
11787 * NOTE: only the attitude update and specific force frame transformation
11788 * phases are precise.
11789 *
11790 * @param timeInterval time interval between epochs.
11791 * @param oldPosition previous curvilinear position expressed in terms of latitude,
11792 * longitude and height.
11793 * @param oldC previous body-to-NED coordinate transformation.
11794 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
11795 * along north, east and down axes.
11796 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
11797 * resolved along body-frame axes, averaged over time interval.
11798 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
11799 * resolved along body-frame axes, averaged over time interval.
11800 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
11801 * resolved along body-frame axes, averaged over time interval.
11802 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
11803 * resolved along body-frame axes, averaged over time interval.
11804 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
11805 * resolved along body-frame axes, averaged over time interval.
11806 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
11807 * resolved along body-frame axes, averaged over time interval.
11808 * @return estimated NED frame containing new body position, velocity and coordinate
11809 * transformation matrix.
11810 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
11811 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
11812 * body-to-NED-frame coordinate transformation matrix are
11813 * invalid.
11814 */
11815 public NEDFrame navigateAndReturnNew(
11816 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
11817 final NEDVelocity oldVelocity, final Acceleration fx, final Acceleration fy, final Acceleration fz,
11818 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
11819 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
11820 return navigateAndReturnNew(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz,
11821 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
11822 }
11823
11824 /**
11825 * Runs precision local-navigation-frame inertial navigation equations.
11826 * NOTE: only the attitude update and specific force frame transformation
11827 * phases are precise.
11828 *
11829 * @param timeInterval time interval between epochs expressed in seconds (s).
11830 * @param oldLatitude previous latitude angle.
11831 * @param oldLongitude previous longitude angle.
11832 * @param oldHeight previous height.
11833 * @param oldC previous body-to-NED coordinate transformation.
11834 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
11835 * resolved along NED-frame axes and expressed in meters per second (m/s).
11836 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
11837 * resolved along NED-frame axes and expressed in meters per second (m/s).
11838 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
11839 * resolved along NED-frame axes and expressed in meters per second (m/s).
11840 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
11841 * resolved along body-frame axes, averaged over time interval.
11842 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
11843 * resolved along body-frame axes, averaged over time interval.
11844 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
11845 * resolved along body-frame axes, averaged over time interval.
11846 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
11847 * resolved along body-frame axes, averaged over time interval.
11848 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
11849 * resolved along body-frame axes, averaged over time interval.
11850 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
11851 * resolved along body-frame axes, averaged over time interval.
11852 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
11853 * @return estimated NED frame containing new body position, velocity and coordinate
11854 * transformation matrix.
11855 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
11856 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
11857 * body-to-NED-frame coordinate transformation matrix are
11858 * invalid.
11859 */
11860 public NEDFrame navigateAndReturnNew(
11861 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
11862 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
11863 final Acceleration fx, final Acceleration fy, final Acceleration fz,
11864 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
11865 final double accuracyThreshold) throws InertialNavigatorException,
11866 InvalidSourceAndDestinationFrameTypeException {
11867 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
11868 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold);
11869 }
11870
11871 /**
11872 * Runs precision local-navigation-frame inertial navigation equations.
11873 * NOTE: only the attitude update and specific force frame transformation
11874 * phases are precise.
11875 *
11876 * @param timeInterval time interval between epochs expressed in seconds (s).
11877 * @param oldLatitude previous latitude angle.
11878 * @param oldLongitude previous longitude angle.
11879 * @param oldHeight previous height.
11880 * @param oldC previous body-to-NED coordinate transformation.
11881 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
11882 * resolved along NED-frame axes and expressed in meters per second (m/s).
11883 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
11884 * resolved along NED-frame axes and expressed in meters per second (m/s).
11885 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
11886 * resolved along NED-frame axes and expressed in meters per second (m/s).
11887 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
11888 * resolved along body-frame axes, averaged over time interval.
11889 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
11890 * resolved along body-frame axes, averaged over time interval.
11891 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
11892 * resolved along body-frame axes, averaged over time interval.
11893 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
11894 * resolved along body-frame axes, averaged over time interval.
11895 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
11896 * resolved along body-frame axes, averaged over time interval.
11897 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
11898 * resolved along body-frame axes, averaged over time interval.
11899 * @return estimated NED frame containing new body position, velocity and coordinate
11900 * transformation matrix.
11901 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
11902 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
11903 * body-to-NED-frame coordinate transformation matrix are
11904 * invalid.
11905 */
11906 public NEDFrame navigateAndReturnNew(
11907 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
11908 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
11909 final Acceleration fx, final Acceleration fy, final Acceleration fz,
11910 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
11911 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
11912 return navigateAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
11913 fx, fy, fz, angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
11914 }
11915
11916 /**
11917 * Runs precision local-navigation-frame inertial navigation equations.
11918 * NOTE: only the attitude update and specific force frame transformation
11919 * phases are precise.
11920 *
11921 * @param timeInterval time interval between epochs.
11922 * @param oldLatitude previous latitude angle.
11923 * @param oldLongitude previous longitude angle.
11924 * @param oldHeight previous height.
11925 * @param oldC previous body-to-NED coordinate transformation.
11926 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
11927 * resolved along NED-frame axes and expressed in meters per second (m/s).
11928 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
11929 * resolved along NED-frame axes and expressed in meters per second (m/s).
11930 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
11931 * resolved along NED-frame axes and expressed in meters per second (m/s).
11932 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
11933 * resolved along body-frame axes, averaged over time interval.
11934 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
11935 * resolved along body-frame axes, averaged over time interval.
11936 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
11937 * resolved along body-frame axes, averaged over time interval.
11938 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
11939 * resolved along body-frame axes, averaged over time interval.
11940 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
11941 * resolved along body-frame axes, averaged over time interval.
11942 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
11943 * resolved along body-frame axes, averaged over time interval.
11944 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
11945 * @return estimated NED frame containing new body position, velocity and coordinate
11946 * transformation matrix.
11947 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
11948 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
11949 * body-to-NED-frame coordinate transformation matrix are
11950 * invalid.
11951 */
11952 public NEDFrame navigateAndReturnNew(
11953 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
11954 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
11955 final Acceleration fx, final Acceleration fy, final Acceleration fz,
11956 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
11957 final double accuracyThreshold) throws InertialNavigatorException,
11958 InvalidSourceAndDestinationFrameTypeException {
11959 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
11960 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold);
11961 }
11962
11963 /**
11964 * Runs precision local-navigation-frame inertial navigation equations.
11965 * NOTE: only the attitude update and specific force frame transformation
11966 * phases are precise.
11967 *
11968 * @param timeInterval time interval between epochs.
11969 * @param oldLatitude previous latitude angle.
11970 * @param oldLongitude previous longitude angle.
11971 * @param oldHeight previous height.
11972 * @param oldC previous body-to-NED coordinate transformation.
11973 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
11974 * resolved along NED-frame axes and expressed in meters per second (m/s).
11975 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
11976 * resolved along NED-frame axes and expressed in meters per second (m/s).
11977 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
11978 * resolved along NED-frame axes and expressed in meters per second (m/s).
11979 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
11980 * resolved along body-frame axes, averaged over time interval.
11981 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
11982 * resolved along body-frame axes, averaged over time interval.
11983 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
11984 * resolved along body-frame axes, averaged over time interval.
11985 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
11986 * resolved along body-frame axes, averaged over time interval.
11987 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
11988 * resolved along body-frame axes, averaged over time interval.
11989 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
11990 * resolved along body-frame axes, averaged over time interval.
11991 * @return estimated NED frame containing new body position, velocity and coordinate
11992 * transformation matrix.
11993 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
11994 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
11995 * body-to-NED-frame coordinate transformation matrix are
11996 * invalid.
11997 */
11998 public NEDFrame navigateAndReturnNew(
11999 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
12000 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
12001 final Acceleration fx, final Acceleration fy, final Acceleration fz,
12002 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
12003 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
12004 return navigateAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
12005 fx, fy, fz, angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
12006 }
12007
12008 /**
12009 * Runs precision local-navigation-frame inertial navigation equations.
12010 * NOTE: only the attitude update and specific force frame transformation
12011 * phases are precise.
12012 *
12013 * @param timeInterval time interval between epochs expressed in seconds (s).
12014 * @param oldPosition previous curvilinear position expressed in terms of latitude,
12015 * longitude and height.
12016 * @param oldC previous body-to-NED coordinate transformation.
12017 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
12018 * resolved along NED-frame axes and expressed in meters per second (m/s).
12019 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
12020 * resolved along NED-frame axes and expressed in meters per second (m/s).
12021 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
12022 * resolved along NED-frame axes and expressed in meters per second (m/s).
12023 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
12024 * resolved along body-frame axes, averaged over time interval.
12025 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
12026 * resolved along body-frame axes, averaged over time interval.
12027 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
12028 * resolved along body-frame axes, averaged over time interval.
12029 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
12030 * resolved along body-frame axes, averaged over time interval.
12031 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
12032 * resolved along body-frame axes, averaged over time interval.
12033 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
12034 * resolved along body-frame axes, averaged over time interval.
12035 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
12036 * @return estimated NED frame containing new body position, velocity and coordinate
12037 * transformation matrix.
12038 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
12039 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
12040 * body-to-NED-frame coordinate transformation matrix are
12041 * invalid.
12042 */
12043 public NEDFrame navigateAndReturnNew(
12044 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
12045 final double oldVn, final double oldVe, final double oldVd,
12046 final Acceleration fx, final Acceleration fy, final Acceleration fz,
12047 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
12048 final double accuracyThreshold) throws InertialNavigatorException,
12049 InvalidSourceAndDestinationFrameTypeException {
12050 return navigateNEDAndReturnNew(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
12051 angularRateX, angularRateY, angularRateZ, accuracyThreshold);
12052 }
12053
12054 /**
12055 * Runs precision local-navigation-frame inertial navigation equations.
12056 * NOTE: only the attitude update and specific force frame transformation
12057 * phases are precise.
12058 *
12059 * @param timeInterval time interval between epochs expressed in seconds (s).
12060 * @param oldPosition previous curvilinear position expressed in terms of latitude,
12061 * longitude and height.
12062 * @param oldC previous body-to-NED coordinate transformation.
12063 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
12064 * resolved along NED-frame axes and expressed in meters per second (m/s).
12065 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
12066 * resolved along NED-frame axes and expressed in meters per second (m/s).
12067 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
12068 * resolved along NED-frame axes and expressed in meters per second (m/s).
12069 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
12070 * resolved along body-frame axes, averaged over time interval.
12071 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
12072 * resolved along body-frame axes, averaged over time interval.
12073 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
12074 * resolved along body-frame axes, averaged over time interval.
12075 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
12076 * resolved along body-frame axes, averaged over time interval.
12077 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
12078 * resolved along body-frame axes, averaged over time interval.
12079 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
12080 * resolved along body-frame axes, averaged over time interval.
12081 * @return estimated NED frame containing new body position, velocity and coordinate
12082 * transformation matrix.
12083 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
12084 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
12085 * body-to-NED-frame coordinate transformation matrix are
12086 * invalid.
12087 */
12088 public NEDFrame navigateAndReturnNew(
12089 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
12090 final double oldVn, final double oldVe, final double oldVd,
12091 final Acceleration fx, final Acceleration fy, final Acceleration fz,
12092 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
12093 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
12094 return navigateAndReturnNew(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
12095 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
12096 }
12097
12098 /**
12099 * Runs precision local-navigation-frame inertial navigation equations.
12100 * NOTE: only the attitude update and specific force frame transformation
12101 * phases are precise.
12102 *
12103 * @param timeInterval time interval between epochs expressed in seconds (s).
12104 * @param oldPosition previous curvilinear position expressed in terms of latitude,
12105 * longitude and height.
12106 * @param oldC previous body-to-NED coordinate transformation.
12107 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
12108 * resolved along NED-frame axes and expressed in meters per second (m/s).
12109 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
12110 * resolved along NED-frame axes and expressed in meters per second (m/s).
12111 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
12112 * resolved along NED-frame axes and expressed in meters per second (m/s).
12113 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
12114 * resolved along body-frame axes, averaged over time interval.
12115 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
12116 * resolved along body-frame axes, averaged over time interval.
12117 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
12118 * resolved along body-frame axes, averaged over time interval.
12119 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
12120 * resolved along body-frame axes, averaged over time interval.
12121 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
12122 * resolved along body-frame axes, averaged over time interval.
12123 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
12124 * resolved along body-frame axes, averaged over time interval.
12125 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
12126 * @return estimated NED frame containing new body position, velocity and coordinate
12127 * transformation matrix.
12128 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
12129 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
12130 * body-to-NED-frame coordinate transformation matrix are
12131 * invalid.
12132 */
12133 public NEDFrame navigateAndReturnNew(
12134 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
12135 final double oldVn, final double oldVe, final double oldVd,
12136 final Acceleration fx, final Acceleration fy, final Acceleration fz,
12137 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
12138 final double accuracyThreshold) throws InertialNavigatorException,
12139 InvalidSourceAndDestinationFrameTypeException {
12140 return navigateNEDAndReturnNew(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
12141 angularRateX, angularRateY, angularRateZ, accuracyThreshold);
12142 }
12143
12144 /**
12145 * Runs precision local-navigation-frame inertial navigation equations.
12146 * NOTE: only the attitude update and specific force frame transformation
12147 * phases are precise.
12148 *
12149 * @param timeInterval time interval between epochs expressed in seconds (s).
12150 * @param oldPosition previous curvilinear position expressed in terms of latitude,
12151 * longitude and height.
12152 * @param oldC previous body-to-NED coordinate transformation.
12153 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
12154 * resolved along NED-frame axes and expressed in meters per second (m/s).
12155 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
12156 * resolved along NED-frame axes and expressed in meters per second (m/s).
12157 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
12158 * resolved along NED-frame axes and expressed in meters per second (m/s).
12159 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
12160 * resolved along body-frame axes, averaged over time interval.
12161 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
12162 * resolved along body-frame axes, averaged over time interval.
12163 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
12164 * resolved along body-frame axes, averaged over time interval.
12165 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
12166 * resolved along body-frame axes, averaged over time interval.
12167 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
12168 * resolved along body-frame axes, averaged over time interval.
12169 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
12170 * resolved along body-frame axes, averaged over time interval.
12171 * @return estimated NED frame containing new body position, velocity and coordinate
12172 * transformation matrix.
12173 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
12174 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
12175 * body-to-NED-frame coordinate transformation matrix are
12176 * invalid.
12177 */
12178 public NEDFrame navigateAndReturnNew(
12179 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
12180 final double oldVn, final double oldVe, final double oldVd,
12181 final Acceleration fx, final Acceleration fy, final Acceleration fz,
12182 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
12183 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
12184 return navigateAndReturnNew(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
12185 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
12186 }
12187
12188 /**
12189 * Runs precision local-navigation-frame inertial navigation equations.
12190 * NOTE: only the attitude update and specific force frame transformation
12191 * phases are precise.
12192 *
12193 * @param timeInterval time interval between epochs expressed in seconds (s).
12194 * @param oldLatitude previous latitude expressed in radians (rad).
12195 * @param oldLongitude previous longitude expressed in radians (rad).
12196 * @param oldHeight previous height expressed in meters (m).
12197 * @param oldC previous body-to-NED coordinate transformation.
12198 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
12199 * resolved along NED-frame axes and expressed in meters per second (m/s).
12200 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
12201 * resolved along NED-frame axes and expressed in meters per second (m/s).
12202 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
12203 * resolved along NED-frame axes and expressed in meters per second (m/s).
12204 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
12205 * resolved along body-frame axes, averaged over time interval.
12206 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
12207 * resolved along body-frame axes, averaged over time interval.
12208 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
12209 * resolved along body-frame axes, averaged over time interval.
12210 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
12211 * resolved along body-frame axes, averaged over time interval.
12212 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
12213 * resolved along body-frame axes, averaged over time interval.
12214 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
12215 * resolved along body-frame axes, averaged over time interval.
12216 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
12217 * @return estimated NED frame containing new body position, velocity and coordinate
12218 * transformation matrix.
12219 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
12220 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
12221 * body-to-NED-frame coordinate transformation matrix are
12222 * invalid.
12223 */
12224 public NEDFrame navigateAndReturnNew(
12225 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
12226 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
12227 final Acceleration fx, final Acceleration fy, final Acceleration fz,
12228 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
12229 final double accuracyThreshold) throws InertialNavigatorException,
12230 InvalidSourceAndDestinationFrameTypeException {
12231 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
12232 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold);
12233 }
12234
12235 /**
12236 * Runs precision local-navigation-frame inertial navigation equations.
12237 * NOTE: only the attitude update and specific force frame transformation
12238 * phases are precise.
12239 *
12240 * @param timeInterval time interval between epochs expressed in seconds (s).
12241 * @param oldLatitude previous latitude expressed in radians (rad).
12242 * @param oldLongitude previous longitude expressed in radians (rad).
12243 * @param oldHeight previous height expressed in meters (m).
12244 * @param oldC previous body-to-NED coordinate transformation.
12245 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
12246 * resolved along NED-frame axes and expressed in meters per second (m/s).
12247 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
12248 * resolved along NED-frame axes and expressed in meters per second (m/s).
12249 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
12250 * resolved along NED-frame axes and expressed in meters per second (m/s).
12251 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
12252 * resolved along body-frame axes, averaged over time interval.
12253 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
12254 * resolved along body-frame axes, averaged over time interval.
12255 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
12256 * resolved along body-frame axes, averaged over time interval.
12257 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
12258 * resolved along body-frame axes, averaged over time interval.
12259 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
12260 * resolved along body-frame axes, averaged over time interval.
12261 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
12262 * resolved along body-frame axes, averaged over time interval.
12263 * @return estimated NED frame containing new body position, velocity and coordinate
12264 * transformation matrix.
12265 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
12266 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
12267 * body-to-NED-frame coordinate transformation matrix are
12268 * invalid.
12269 */
12270 public NEDFrame navigateAndReturnNew(
12271 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
12272 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
12273 final Acceleration fx, final Acceleration fy, final Acceleration fz,
12274 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
12275 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
12276 return navigateAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
12277 fx, fy, fz, angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
12278 }
12279
12280 /**
12281 * Runs precision local-navigation-frame inertial navigation equations.
12282 * NOTE: only the attitude update and specific force frame transformation
12283 * phases are precise.
12284 *
12285 * @param timeInterval time interval between epochs.
12286 * @param oldLatitude previous latitude expressed in radians (rad).
12287 * @param oldLongitude previous longitude expressed in radians (rad).
12288 * @param oldHeight previous height expressed in meters (m).
12289 * @param oldC previous body-to-NED coordinate transformation.
12290 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
12291 * resolved along NED-frame axes and expressed in meters per second (m/s).
12292 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
12293 * resolved along NED-frame axes and expressed in meters per second (m/s).
12294 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
12295 * resolved along NED-frame axes and expressed in meters per second (m/s).
12296 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
12297 * resolved along body-frame axes, averaged over time interval.
12298 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
12299 * resolved along body-frame axes, averaged over time interval.
12300 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
12301 * resolved along body-frame axes, averaged over time interval.
12302 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
12303 * resolved along body-frame axes, averaged over time interval.
12304 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
12305 * resolved along body-frame axes, averaged over time interval.
12306 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
12307 * resolved along body-frame axes, averaged over time interval.
12308 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
12309 * @return estimated NED frame containing new body position, velocity and coordinate
12310 * transformation matrix.
12311 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
12312 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
12313 * body-to-NED-frame coordinate transformation matrix are
12314 * invalid.
12315 */
12316 public NEDFrame navigateAndReturnNew(
12317 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
12318 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
12319 final Acceleration fx, final Acceleration fy, final Acceleration fz,
12320 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
12321 final double accuracyThreshold) throws InertialNavigatorException,
12322 InvalidSourceAndDestinationFrameTypeException {
12323 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
12324 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold);
12325 }
12326
12327 /**
12328 * Runs precision local-navigation-frame inertial navigation equations.
12329 * NOTE: only the attitude update and specific force frame transformation
12330 * phases are precise.
12331 *
12332 * @param timeInterval time interval between epochs.
12333 * @param oldLatitude previous latitude expressed in radians (rad).
12334 * @param oldLongitude previous longitude expressed in radians (rad).
12335 * @param oldHeight previous height expressed in meters (m).
12336 * @param oldC previous body-to-NED coordinate transformation.
12337 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
12338 * resolved along NED-frame axes and expressed in meters per second (m/s).
12339 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
12340 * resolved along NED-frame axes and expressed in meters per second (m/s).
12341 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
12342 * resolved along NED-frame axes and expressed in meters per second (m/s).
12343 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
12344 * resolved along body-frame axes, averaged over time interval.
12345 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
12346 * resolved along body-frame axes, averaged over time interval.
12347 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
12348 * resolved along body-frame axes, averaged over time interval.
12349 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
12350 * resolved along body-frame axes, averaged over time interval.
12351 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
12352 * resolved along body-frame axes, averaged over time interval.
12353 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
12354 * resolved along body-frame axes, averaged over time interval.
12355 * @return estimated NED frame containing new body position, velocity and coordinate
12356 * transformation matrix.
12357 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
12358 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
12359 * body-to-NED-frame coordinate transformation matrix are
12360 * invalid.
12361 */
12362 public NEDFrame navigateAndReturnNew(
12363 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
12364 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
12365 final Acceleration fx, final Acceleration fy, final Acceleration fz,
12366 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
12367 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
12368 return navigateAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
12369 fx, fy, fz, angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
12370 }
12371
12372 /**
12373 * Runs precision local-navigation-frame inertial navigation equations.
12374 * NOTE: only the attitude update and specific force frame transformation
12375 * phases are precise.
12376 *
12377 * @param timeInterval time interval between epochs expressed in seconds (s).
12378 * @param oldLatitude previous latitude angle.
12379 * @param oldLongitude previous longitude angle.
12380 * @param oldHeight previous height.
12381 * @param oldC previous body-to-NED coordinate transformation.
12382 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
12383 * resolved along NED-frame axes.
12384 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
12385 * resolved along NED-frame axes.
12386 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
12387 * resolved along NED-frame axes.
12388 * @param kinematics body kinematics containing specific forces and angular rates applied to
12389 * the body.
12390 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
12391 * @return estimated NED frame containing new body position, velocity and coordinate
12392 * transformation matrix.
12393 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
12394 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
12395 * body-to-NED-frame coordinate transformation matrix are
12396 * invalid.
12397 */
12398 public NEDFrame navigateAndReturnNew(
12399 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
12400 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
12401 final BodyKinematics kinematics, final double accuracyThreshold) throws InertialNavigatorException,
12402 InvalidSourceAndDestinationFrameTypeException {
12403 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
12404 oldSpeedN, oldSpeedE, oldSpeedD, kinematics, accuracyThreshold);
12405 }
12406
12407 /**
12408 * Runs precision local-navigation-frame inertial navigation equations.
12409 * NOTE: only the attitude update and specific force frame transformation
12410 * phases are precise.
12411 *
12412 * @param timeInterval time interval between epochs expressed in seconds (s).
12413 * @param oldLatitude previous latitude angle.
12414 * @param oldLongitude previous longitude angle.
12415 * @param oldHeight previous height.
12416 * @param oldC previous body-to-NED coordinate transformation.
12417 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
12418 * resolved along NED-frame axes.
12419 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
12420 * resolved along NED-frame axes.
12421 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
12422 * resolved along NED-frame axes.
12423 * @param kinematics body kinematics containing specific forces and angular rates applied to
12424 * the body.
12425 * @return estimated NED frame containing new body position, velocity and coordinate
12426 * transformation matrix.
12427 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
12428 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
12429 * body-to-NED-frame coordinate transformation matrix are
12430 * invalid.
12431 */
12432 public NEDFrame navigateAndReturnNew(
12433 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
12434 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
12435 final BodyKinematics kinematics) throws InertialNavigatorException,
12436 InvalidSourceAndDestinationFrameTypeException {
12437 return navigateAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
12438 oldSpeedN, oldSpeedE, oldSpeedD, kinematics, DEFAULT_ACCURACY_THRESHOLD);
12439 }
12440
12441 /**
12442 * Runs precision local-navigation-frame inertial navigation equations.
12443 * NOTE: only the attitude update and specific force frame transformation
12444 * phases are precise.
12445 *
12446 * @param timeInterval time interval between epochs.
12447 * @param oldLatitude previous latitude angle.
12448 * @param oldLongitude previous longitude angle.
12449 * @param oldHeight previous height.
12450 * @param oldC previous body-to-NED coordinate transformation.
12451 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
12452 * resolved along NED-frame axes.
12453 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
12454 * resolved along NED-frame axes.
12455 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
12456 * resolved along NED-frame axes.
12457 * @param kinematics body kinematics containing specific forces and angular rates applied to
12458 * the body.
12459 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
12460 * @return estimated NED frame containing new body position, velocity and coordinate
12461 * transformation matrix.
12462 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
12463 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
12464 * body-to-NED-frame coordinate transformation matrix are
12465 * invalid.
12466 */
12467 public NEDFrame navigateAndReturnNew(
12468 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
12469 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
12470 final BodyKinematics kinematics, final double accuracyThreshold) throws InertialNavigatorException,
12471 InvalidSourceAndDestinationFrameTypeException {
12472 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
12473 oldSpeedN, oldSpeedE, oldSpeedD, kinematics, accuracyThreshold);
12474 }
12475
12476 /**
12477 * Runs precision local-navigation-frame inertial navigation equations.
12478 * NOTE: only the attitude update and specific force frame transformation
12479 * phases are precise.
12480 *
12481 * @param timeInterval time interval between epochs.
12482 * @param oldLatitude previous latitude angle.
12483 * @param oldLongitude previous longitude angle.
12484 * @param oldHeight previous height.
12485 * @param oldC previous body-to-NED coordinate transformation.
12486 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
12487 * resolved along NED-frame axes.
12488 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
12489 * resolved along NED-frame axes.
12490 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
12491 * resolved along NED-frame axes.
12492 * @param kinematics body kinematics containing specific forces and angular rates applied to
12493 * the body.
12494 * @return estimated NED frame containing new body position, velocity and coordinate
12495 * transformation matrix.
12496 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
12497 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
12498 * body-to-NED-frame coordinate transformation matrix are
12499 * invalid.
12500 */
12501 public NEDFrame navigateAndReturnNew(
12502 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
12503 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
12504 final BodyKinematics kinematics) throws InertialNavigatorException,
12505 InvalidSourceAndDestinationFrameTypeException {
12506 return navigateAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
12507 oldSpeedN, oldSpeedE, oldSpeedD, kinematics, DEFAULT_ACCURACY_THRESHOLD);
12508 }
12509
12510 /**
12511 * Runs precision local-navigation-frame inertial navigation equations.
12512 * NOTE: only the attitude update and specific force frame transformation
12513 * phases are precise.
12514 *
12515 * @param timeInterval time interval between epochs expressed in seconds (s).
12516 * @param oldFrame previous NED frame containing body position, velocity and
12517 * coordinate transformation matrix.
12518 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
12519 * resolved along body-frame axes, averaged over time interval and
12520 * expressed in meters per squared second (m/s^2).
12521 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
12522 * resolved along body-frame axes, averaged over time interval and
12523 * expressed in meters per squared second (m/s^2).
12524 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
12525 * resolved along body-frame axes, averaged over time interval and
12526 * expressed in meters per squared second (m/s^2).
12527 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
12528 * resolved along body-frame axes, averaged over time interval and
12529 * expressed in radians per second (rad/s).
12530 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
12531 * resolved along body-frame axes, averaged over time interval and
12532 * expressed in radians per second (rad/s).
12533 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
12534 * resolved along body-frame axes, averaged over time interval and
12535 * expressed in radians per second (rad/s).
12536 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
12537 * @return estimated NED frame containing new body position, velocity and coordinate
12538 * transformation matrix.
12539 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
12540 */
12541 public NEDFrame navigateAndReturnNew(
12542 final double timeInterval, final NEDFrame oldFrame, final double fx, final double fy, final double fz,
12543 final double angularRateX, final double angularRateY, final double angularRateZ,
12544 final double accuracyThreshold) throws InertialNavigatorException {
12545 return navigateNEDAndReturnNew(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
12546 accuracyThreshold);
12547 }
12548
12549 /**
12550 * Runs precision local-navigation-frame inertial navigation equations.
12551 * NOTE: only the attitude update and specific force frame transformation
12552 * phases are precise.
12553 *
12554 * @param timeInterval time interval between epochs expressed in seconds (s).
12555 * @param oldFrame previous NED frame containing body position, velocity and
12556 * coordinate transformation matrix.
12557 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
12558 * resolved along body-frame axes, averaged over time interval and
12559 * expressed in meters per squared second (m/s^2).
12560 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
12561 * resolved along body-frame axes, averaged over time interval and
12562 * expressed in meters per squared second (m/s^2).
12563 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
12564 * resolved along body-frame axes, averaged over time interval and
12565 * expressed in meters per squared second (m/s^2).
12566 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
12567 * resolved along body-frame axes, averaged over time interval and
12568 * expressed in radians per second (rad/s).
12569 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
12570 * resolved along body-frame axes, averaged over time interval and
12571 * expressed in radians per second (rad/s).
12572 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
12573 * resolved along body-frame axes, averaged over time interval and
12574 * expressed in radians per second (rad/s).
12575 * @return estimated NED frame containing new body position, velocity and coordinate
12576 * transformation matrix.
12577 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
12578 */
12579 public NEDFrame navigateAndReturnNew(
12580 final double timeInterval, final NEDFrame oldFrame, final double fx, final double fy, final double fz,
12581 final double angularRateX, final double angularRateY, final double angularRateZ)
12582 throws InertialNavigatorException {
12583 return navigateAndReturnNew(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
12584 DEFAULT_ACCURACY_THRESHOLD);
12585 }
12586
12587 /**
12588 * Runs precision local-navigation-frame inertial navigation equations.
12589 * NOTE: only the attitude update and specific force frame transformation
12590 * phases are precise.
12591 *
12592 * @param timeInterval time interval between epochs.
12593 * @param oldFrame previous NED frame containing body position, velocity and
12594 * coordinate transformation matrix.
12595 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
12596 * resolved along body-frame axes, averaged over time interval and
12597 * expressed in meters per squared second (m/s^2).
12598 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
12599 * resolved along body-frame axes, averaged over time interval and
12600 * expressed in meters per squared second (m/s^2).
12601 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
12602 * resolved along body-frame axes, averaged over time interval and
12603 * expressed in meters per squared second (m/s^2).
12604 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
12605 * resolved along body-frame axes, averaged over time interval and
12606 * expressed in radians per second (rad/s).
12607 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
12608 * resolved along body-frame axes, averaged over time interval and
12609 * expressed in radians per second (rad/s).
12610 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
12611 * resolved along body-frame axes, averaged over time interval and
12612 * expressed in radians per second (rad/s).
12613 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
12614 * @return estimated NED frame containing new body position, velocity and coordinate
12615 * transformation matrix.
12616 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
12617 */
12618 public NEDFrame navigateAndReturnNew(
12619 final Time timeInterval, final NEDFrame oldFrame, final double fx, final double fy, final double fz,
12620 final double angularRateX, final double angularRateY, final double angularRateZ,
12621 final double accuracyThreshold) throws InertialNavigatorException {
12622 return navigateNEDAndReturnNew(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
12623 accuracyThreshold);
12624 }
12625
12626 /**
12627 * Runs precision local-navigation-frame inertial navigation equations.
12628 * NOTE: only the attitude update and specific force frame transformation
12629 * phases are precise.
12630 *
12631 * @param timeInterval time interval between epochs.
12632 * @param oldFrame previous NED frame containing body position, velocity and
12633 * coordinate transformation matrix.
12634 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
12635 * resolved along body-frame axes, averaged over time interval and
12636 * expressed in meters per squared second (m/s^2).
12637 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
12638 * resolved along body-frame axes, averaged over time interval and
12639 * expressed in meters per squared second (m/s^2).
12640 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
12641 * resolved along body-frame axes, averaged over time interval and
12642 * expressed in meters per squared second (m/s^2).
12643 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
12644 * resolved along body-frame axes, averaged over time interval and
12645 * expressed in radians per second (rad/s).
12646 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
12647 * resolved along body-frame axes, averaged over time interval and
12648 * expressed in radians per second (rad/s).
12649 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
12650 * resolved along body-frame axes, averaged over time interval and
12651 * expressed in radians per second (rad/s).
12652 * @return estimated NED frame containing new body position, velocity and coordinate
12653 * transformation matrix.
12654 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
12655 */
12656 public NEDFrame navigateAndReturnNew(
12657 final Time timeInterval, final NEDFrame oldFrame, final double fx, final double fy, final double fz,
12658 final double angularRateX, final double angularRateY, final double angularRateZ)
12659 throws InertialNavigatorException {
12660 return navigateAndReturnNew(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
12661 DEFAULT_ACCURACY_THRESHOLD);
12662 }
12663
12664 /**
12665 * Runs precision local-navigation-frame inertial navigation equations.
12666 * NOTE: only the attitude update and specific force frame transformation
12667 * phases are precise.
12668 *
12669 * @param timeInterval time interval between epochs expressed in seconds (s).
12670 * @param oldFrame previous NED frame containing body position, velocity and
12671 * coordinate transformation matrix.
12672 * @param kinematics body kinematics containing specific forces and angular rates applied to
12673 * the body.
12674 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
12675 * @return estimated NED frame containing new body position, velocity and coordinate
12676 * transformation matrix.
12677 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
12678 */
12679 public NEDFrame navigateAndReturnNew(
12680 final double timeInterval, final NEDFrame oldFrame, final BodyKinematics kinematics,
12681 final double accuracyThreshold) throws InertialNavigatorException {
12682 return navigateNEDAndReturnNew(timeInterval, oldFrame, kinematics, accuracyThreshold);
12683 }
12684
12685 /**
12686 * Runs precision local-navigation-frame inertial navigation equations.
12687 * NOTE: only the attitude update and specific force frame transformation
12688 * phases are precise.
12689 *
12690 * @param timeInterval time interval between epochs expressed in seconds (s).
12691 * @param oldFrame previous NED frame containing body position, velocity and
12692 * coordinate transformation matrix.
12693 * @param kinematics body kinematics containing specific forces and angular rates applied to
12694 * the body.
12695 * @return estimated NED frame containing new body position, velocity and coordinate
12696 * transformation matrix.
12697 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
12698 */
12699 public NEDFrame navigateAndReturnNew(
12700 final double timeInterval, final NEDFrame oldFrame, final BodyKinematics kinematics)
12701 throws InertialNavigatorException {
12702 return navigateAndReturnNew(timeInterval, oldFrame, kinematics, DEFAULT_ACCURACY_THRESHOLD);
12703 }
12704
12705 /**
12706 * Runs precision local-navigation-frame inertial navigation equations.
12707 * NOTE: only the attitude update and specific force frame transformation
12708 * phases are precise.
12709 *
12710 * @param timeInterval time interval between epochs.
12711 * @param oldFrame previous NED frame containing body position, velocity and
12712 * coordinate transformation matrix.
12713 * @param kinematics body kinematics containing specific forces and angular rates applied to
12714 * the body.
12715 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
12716 * @return estimated NED frame containing new body position, velocity and coordinate
12717 * transformation matrix.
12718 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
12719 */
12720 public NEDFrame navigateAndReturnNew(
12721 final Time timeInterval, final NEDFrame oldFrame, final BodyKinematics kinematics,
12722 final double accuracyThreshold) throws InertialNavigatorException {
12723 return navigateNEDAndReturnNew(timeInterval, oldFrame, kinematics, accuracyThreshold);
12724 }
12725
12726 /**
12727 * Runs precision local-navigation-frame inertial navigation equations.
12728 * NOTE: only the attitude update and specific force frame transformation
12729 * phases are precise.
12730 *
12731 * @param timeInterval time interval between epochs.
12732 * @param oldFrame previous NED frame containing body position, velocity and
12733 * coordinate transformation matrix.
12734 * @param kinematics body kinematics containing specific forces and angular rates applied to
12735 * the body.
12736 * @return estimated NED frame containing new body position, velocity and coordinate
12737 * transformation matrix.
12738 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
12739 */
12740 public NEDFrame navigateAndReturnNew(
12741 final Time timeInterval, final NEDFrame oldFrame, final BodyKinematics kinematics)
12742 throws InertialNavigatorException {
12743 return navigateAndReturnNew(timeInterval, oldFrame, kinematics, DEFAULT_ACCURACY_THRESHOLD);
12744 }
12745
12746 /**
12747 * Runs precision local-navigation-frame inertial navigation equations.
12748 * NOTE: only the attitude update and specific force frame transformation
12749 * phases are precise.
12750 *
12751 * @param timeInterval time interval between epochs expressed in seconds (s).
12752 * @param oldFrame previous NED frame containing body position, velocity and
12753 * coordinate transformation matrix.
12754 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
12755 * resolved along body-frame axes, averaged over time interval.
12756 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
12757 * resolved along body-frame axes, averaged over time interval.
12758 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
12759 * resolved along body-frame axes, averaged over time interval.
12760 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
12761 * resolved along body-frame axes, averaged over time interval and
12762 * expressed in radians per second (rad/s).
12763 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
12764 * resolved along body-frame axes, averaged over time interval and
12765 * expressed in radians per second (rad/s).
12766 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
12767 * resolved along body-frame axes, averaged over time interval and
12768 * expressed in radians per second (rad/s).
12769 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
12770 * @return estimated NED frame containing new body position, velocity and coordinate
12771 * transformation matrix.
12772 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
12773 */
12774 public NEDFrame navigateAndReturnNew(
12775 final double timeInterval, final NEDFrame oldFrame,
12776 final Acceleration fx, final Acceleration fy, final Acceleration fz,
12777 final double angularRateX, final double angularRateY, final double angularRateZ,
12778 final double accuracyThreshold) throws InertialNavigatorException {
12779 return navigateNEDAndReturnNew(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
12780 accuracyThreshold);
12781 }
12782
12783 /**
12784 * Runs precision local-navigation-frame inertial navigation equations.
12785 * NOTE: only the attitude update and specific force frame transformation
12786 * phases are precise.
12787 *
12788 * @param timeInterval time interval between epochs expressed in seconds (s).
12789 * @param oldFrame previous NED frame containing body position, velocity and
12790 * coordinate transformation matrix.
12791 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
12792 * resolved along body-frame axes, averaged over time interval.
12793 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
12794 * resolved along body-frame axes, averaged over time interval.
12795 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
12796 * resolved along body-frame axes, averaged over time interval.
12797 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
12798 * resolved along body-frame axes, averaged over time interval and
12799 * expressed in radians per second (rad/s).
12800 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
12801 * resolved along body-frame axes, averaged over time interval and
12802 * expressed in radians per second (rad/s).
12803 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
12804 * resolved along body-frame axes, averaged over time interval and
12805 * expressed in radians per second (rad/s).
12806 * @return estimated NED frame containing new body position, velocity and coordinate
12807 * transformation matrix.
12808 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
12809 */
12810 public NEDFrame navigateAndReturnNew(
12811 final double timeInterval, final NEDFrame oldFrame,
12812 final Acceleration fx, final Acceleration fy, final Acceleration fz,
12813 final double angularRateX, final double angularRateY, final double angularRateZ)
12814 throws InertialNavigatorException {
12815 return navigateAndReturnNew(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
12816 DEFAULT_ACCURACY_THRESHOLD);
12817 }
12818
12819 /**
12820 * Runs precision local-navigation-frame inertial navigation equations.
12821 * NOTE: only the attitude update and specific force frame transformation
12822 * phases are precise.
12823 *
12824 * @param timeInterval time interval between epochs.
12825 * @param oldFrame previous NED frame containing body position, velocity and
12826 * coordinate transformation matrix.
12827 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
12828 * resolved along body-frame axes, averaged over time interval.
12829 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
12830 * resolved along body-frame axes, averaged over time interval.
12831 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
12832 * resolved along body-frame axes, averaged over time interval.
12833 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
12834 * resolved along body-frame axes, averaged over time interval and
12835 * expressed in radians per second (rad/s).
12836 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
12837 * resolved along body-frame axes, averaged over time interval and
12838 * expressed in radians per second (rad/s).
12839 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
12840 * resolved along body-frame axes, averaged over time interval and
12841 * expressed in radians per second (rad/s).
12842 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
12843 * @return estimated NED frame containing new body position, velocity and coordinate
12844 * transformation matrix.
12845 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
12846 */
12847 public NEDFrame navigateAndReturnNew(
12848 final Time timeInterval, final NEDFrame oldFrame,
12849 final Acceleration fx, final Acceleration fy, final Acceleration fz,
12850 final double angularRateX, final double angularRateY, final double angularRateZ,
12851 final double accuracyThreshold) throws InertialNavigatorException {
12852 return navigateNEDAndReturnNew(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
12853 accuracyThreshold);
12854 }
12855
12856 /**
12857 * Runs precision local-navigation-frame inertial navigation equations.
12858 * NOTE: only the attitude update and specific force frame transformation
12859 * phases are precise.
12860 *
12861 * @param timeInterval time interval between epochs.
12862 * @param oldFrame previous NED frame containing body position, velocity and
12863 * coordinate transformation matrix.
12864 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
12865 * resolved along body-frame axes, averaged over time interval.
12866 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
12867 * resolved along body-frame axes, averaged over time interval.
12868 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
12869 * resolved along body-frame axes, averaged over time interval.
12870 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
12871 * resolved along body-frame axes, averaged over time interval and
12872 * expressed in radians per second (rad/s).
12873 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
12874 * resolved along body-frame axes, averaged over time interval and
12875 * expressed in radians per second (rad/s).
12876 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
12877 * resolved along body-frame axes, averaged over time interval and
12878 * expressed in radians per second (rad/s).
12879 * @return estimated NED frame containing new body position, velocity and coordinate
12880 * transformation matrix.
12881 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
12882 */
12883 public NEDFrame navigateAndReturnNew(
12884 final Time timeInterval, final NEDFrame oldFrame,
12885 final Acceleration fx, final Acceleration fy, final Acceleration fz,
12886 final double angularRateX, final double angularRateY, final double angularRateZ)
12887 throws InertialNavigatorException {
12888 return navigateAndReturnNew(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
12889 DEFAULT_ACCURACY_THRESHOLD);
12890 }
12891
12892 /**
12893 * Runs precision local-navigation-frame inertial navigation equations.
12894 * NOTE: only the attitude update and specific force frame transformation
12895 * phases are precise.
12896 *
12897 * @param timeInterval time interval between epochs expressed in seconds (s).
12898 * @param oldFrame previous NED frame containing body position, velocity and
12899 * coordinate transformation matrix.
12900 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
12901 * resolved along body-frame axes, averaged over time interval and
12902 * expressed in meters per squared second (m/s^2).
12903 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
12904 * resolved along body-frame axes, averaged over time interval and
12905 * expressed in meters per squared second (m/s^2).
12906 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
12907 * resolved along body-frame axes, averaged over time interval and
12908 * expressed in meters per squared second (m/s^2).
12909 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
12910 * resolved along body-frame axes, averaged over time interval.
12911 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
12912 * resolved along body-frame axes, averaged over time interval.
12913 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
12914 * resolved along body-frame axes, averaged over time interval.
12915 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
12916 * @return estimated NED frame containing new body position, velocity and coordinate
12917 * transformation matrix.
12918 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
12919 */
12920 public NEDFrame navigateAndReturnNew(
12921 final double timeInterval, final NEDFrame oldFrame, final double fx, final double fy, final double fz,
12922 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
12923 final double accuracyThreshold) throws InertialNavigatorException {
12924 return navigateNEDAndReturnNew(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
12925 accuracyThreshold);
12926 }
12927
12928 /**
12929 * Runs precision local-navigation-frame inertial navigation equations.
12930 * NOTE: only the attitude update and specific force frame transformation
12931 * phases are precise.
12932 *
12933 * @param timeInterval time interval between epochs expressed in seconds (s).
12934 * @param oldFrame previous NED frame containing body position, velocity and
12935 * coordinate transformation matrix.
12936 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
12937 * resolved along body-frame axes, averaged over time interval and
12938 * expressed in meters per squared second (m/s^2).
12939 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
12940 * resolved along body-frame axes, averaged over time interval and
12941 * expressed in meters per squared second (m/s^2).
12942 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
12943 * resolved along body-frame axes, averaged over time interval and
12944 * expressed in meters per squared second (m/s^2).
12945 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
12946 * resolved along body-frame axes, averaged over time interval.
12947 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
12948 * resolved along body-frame axes, averaged over time interval.
12949 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
12950 * resolved along body-frame axes, averaged over time interval.
12951 * @return estimated NED frame containing new body position, velocity and coordinate
12952 * transformation matrix.
12953 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
12954 */
12955 public NEDFrame navigateAndReturnNew(
12956 final double timeInterval, final NEDFrame oldFrame, final double fx, final double fy, final double fz,
12957 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
12958 throws InertialNavigatorException {
12959 return navigateAndReturnNew(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
12960 DEFAULT_ACCURACY_THRESHOLD);
12961 }
12962
12963 /**
12964 * Runs precision local-navigation-frame inertial navigation equations.
12965 * NOTE: only the attitude update and specific force frame transformation
12966 * phases are precise.
12967 *
12968 * @param timeInterval time interval between epochs.
12969 * @param oldFrame previous NED frame containing body position, velocity and
12970 * coordinate transformation matrix.
12971 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
12972 * resolved along body-frame axes, averaged over time interval and
12973 * expressed in meters per squared second (m/s^2).
12974 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
12975 * resolved along body-frame axes, averaged over time interval and
12976 * expressed in meters per squared second (m/s^2).
12977 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
12978 * resolved along body-frame axes, averaged over time interval and
12979 * expressed in meters per squared second (m/s^2).
12980 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
12981 * resolved along body-frame axes, averaged over time interval.
12982 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
12983 * resolved along body-frame axes, averaged over time interval.
12984 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
12985 * resolved along body-frame axes, averaged over time interval.
12986 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
12987 * @return estimated NED frame containing new body position, velocity and coordinate
12988 * transformation matrix.
12989 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
12990 */
12991 public NEDFrame navigateAndReturnNew(
12992 final Time timeInterval, final NEDFrame oldFrame, final double fx, final double fy, final double fz,
12993 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
12994 final double accuracyThreshold) throws InertialNavigatorException {
12995 return navigateNEDAndReturnNew(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
12996 accuracyThreshold);
12997 }
12998
12999 /**
13000 * Runs precision local-navigation-frame inertial navigation equations.
13001 * NOTE: only the attitude update and specific force frame transformation
13002 * phases are precise.
13003 *
13004 * @param timeInterval time interval between epochs.
13005 * @param oldFrame previous NED frame containing body position, velocity and
13006 * coordinate transformation matrix.
13007 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
13008 * resolved along body-frame axes, averaged over time interval and
13009 * expressed in meters per squared second (m/s^2).
13010 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
13011 * resolved along body-frame axes, averaged over time interval and
13012 * expressed in meters per squared second (m/s^2).
13013 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
13014 * resolved along body-frame axes, averaged over time interval and
13015 * expressed in meters per squared second (m/s^2).
13016 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
13017 * resolved along body-frame axes, averaged over time interval.
13018 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
13019 * resolved along body-frame axes, averaged over time interval.
13020 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
13021 * resolved along body-frame axes, averaged over time interval.
13022 * @return estimated NED frame containing new body position, velocity and coordinate
13023 * transformation matrix.
13024 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
13025 */
13026 public NEDFrame navigateAndReturnNew(
13027 final Time timeInterval, final NEDFrame oldFrame, final double fx, final double fy, final double fz,
13028 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
13029 throws InertialNavigatorException {
13030 return navigateAndReturnNew(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
13031 DEFAULT_ACCURACY_THRESHOLD);
13032 }
13033
13034 /**
13035 * Runs precision local-navigation-frame inertial navigation equations.
13036 * NOTE: only the attitude update and specific force frame transformation
13037 * phases are precise.
13038 *
13039 * @param timeInterval time interval between epochs expressed in seconds (s).
13040 * @param oldFrame previous NED frame containing body position, velocity and
13041 * coordinate transformation matrix.
13042 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
13043 * resolved along body-frame axes, averaged over time interval and
13044 * expressed in meters per squared second (m/s^2).
13045 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
13046 * resolved along body-frame axes, averaged over time interval and
13047 * expressed in meters per squared second (m/s^2).
13048 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
13049 * resolved along body-frame axes, averaged over time interval and
13050 * expressed in meters per squared second (m/s^2).
13051 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
13052 * resolved along body-frame axes, averaged over time interval.
13053 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
13054 * resolved along body-frame axes, averaged over time interval.
13055 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
13056 * resolved along body-frame axes, averaged over time interval.
13057 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
13058 * @return estimated NED frame containing new body position, velocity and coordinate
13059 * transformation matrix.
13060 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
13061 */
13062 public NEDFrame navigateAndReturnNew(
13063 final double timeInterval, final NEDFrame oldFrame,
13064 final Acceleration fx, final Acceleration fy, final Acceleration fz,
13065 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
13066 final double accuracyThreshold) throws InertialNavigatorException {
13067 return navigateNEDAndReturnNew(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
13068 accuracyThreshold);
13069 }
13070
13071 /**
13072 * Runs precision local-navigation-frame inertial navigation equations.
13073 * NOTE: only the attitude update and specific force frame transformation
13074 * phases are precise.
13075 *
13076 * @param timeInterval time interval between epochs expressed in seconds (s).
13077 * @param oldFrame previous NED frame containing body position, velocity and
13078 * coordinate transformation matrix.
13079 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
13080 * resolved along body-frame axes, averaged over time interval and
13081 * expressed in meters per squared second (m/s^2).
13082 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
13083 * resolved along body-frame axes, averaged over time interval and
13084 * expressed in meters per squared second (m/s^2).
13085 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
13086 * resolved along body-frame axes, averaged over time interval and
13087 * expressed in meters per squared second (m/s^2).
13088 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
13089 * resolved along body-frame axes, averaged over time interval.
13090 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
13091 * resolved along body-frame axes, averaged over time interval.
13092 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
13093 * resolved along body-frame axes, averaged over time interval.
13094 * @return estimated NED frame containing new body position, velocity and coordinate
13095 * transformation matrix.
13096 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
13097 */
13098 public NEDFrame navigateAndReturnNew(
13099 final double timeInterval, final NEDFrame oldFrame,
13100 final Acceleration fx, final Acceleration fy, final Acceleration fz,
13101 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
13102 throws InertialNavigatorException {
13103 return navigateAndReturnNew(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
13104 DEFAULT_ACCURACY_THRESHOLD);
13105 }
13106
13107 /**
13108 * Runs precision local-navigation-frame inertial navigation equations.
13109 * NOTE: only the attitude update and specific force frame transformation
13110 * phases are precise.
13111 *
13112 * @param timeInterval time interval between epochs expressed in seconds (s).
13113 * @param oldFrame previous NED frame containing body position, velocity and
13114 * coordinate transformation matrix.
13115 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
13116 * resolved along body-frame axes, averaged over time interval and
13117 * expressed in meters per squared second (m/s^2).
13118 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
13119 * resolved along body-frame axes, averaged over time interval and
13120 * expressed in meters per squared second (m/s^2).
13121 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
13122 * resolved along body-frame axes, averaged over time interval and
13123 * expressed in meters per squared second (m/s^2).
13124 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
13125 * resolved along body-frame axes, averaged over time interval.
13126 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
13127 * resolved along body-frame axes, averaged over time interval.
13128 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
13129 * resolved along body-frame axes, averaged over time interval.
13130 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
13131 * @return estimated NED frame containing new body position, velocity and coordinate
13132 * transformation matrix.
13133 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
13134 */
13135 public NEDFrame navigateAndReturnNew(
13136 final Time timeInterval, final NEDFrame oldFrame,
13137 final Acceleration fx, final Acceleration fy, final Acceleration fz,
13138 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
13139 final double accuracyThreshold) throws InertialNavigatorException {
13140 return navigateNEDAndReturnNew(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
13141 accuracyThreshold);
13142 }
13143
13144 /**
13145 * Runs precision local-navigation-frame inertial navigation equations.
13146 * NOTE: only the attitude update and specific force frame transformation
13147 * phases are precise.
13148 *
13149 * @param timeInterval time interval between epochs expressed in seconds (s).
13150 * @param oldFrame previous NED frame containing body position, velocity and
13151 * coordinate transformation matrix.
13152 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
13153 * resolved along body-frame axes, averaged over time interval and
13154 * expressed in meters per squared second (m/s^2).
13155 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
13156 * resolved along body-frame axes, averaged over time interval and
13157 * expressed in meters per squared second (m/s^2).
13158 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
13159 * resolved along body-frame axes, averaged over time interval and
13160 * expressed in meters per squared second (m/s^2).
13161 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
13162 * resolved along body-frame axes, averaged over time interval.
13163 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
13164 * resolved along body-frame axes, averaged over time interval.
13165 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
13166 * resolved along body-frame axes, averaged over time interval.
13167 * @return estimated NED frame containing new body position, velocity and coordinate
13168 * transformation matrix.
13169 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
13170 */
13171 public NEDFrame navigateAndReturnNew(
13172 final Time timeInterval, final NEDFrame oldFrame,
13173 final Acceleration fx, final Acceleration fy, final Acceleration fz,
13174 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
13175 throws InertialNavigatorException {
13176 return navigateAndReturnNew(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
13177 DEFAULT_ACCURACY_THRESHOLD);
13178 }
13179
13180 /**
13181 * Runs precision local-navigation-frame inertial navigation equations.
13182 * NOTE: only the attitude update and specific force frame transformation
13183 * phases are precise.
13184 *
13185 * @param timeInterval time interval between epochs expressed in seconds (s).
13186 * @param oldLatitude previous latitude expressed in radians (rad).
13187 * @param oldLongitude previous longitude expressed in radians (rad).
13188 * @param oldHeight previous height expressed in meters (m).
13189 * @param oldC previous body-to-NED coordinate transformation.
13190 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
13191 * resolved along NED-frame axes and expressed in meters per second (m/s).
13192 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
13193 * resolved along NED-frame axes and expressed in meters per second (m/s).
13194 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
13195 * resolved along NED-frame axes and expressed in meters per second (m/s).
13196 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
13197 * resolved along body-frame axes, averaged over time interval and
13198 * expressed in meters per squared second (m/s^2).
13199 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
13200 * resolved along body-frame axes, averaged over time interval and
13201 * expressed in meters per squared second (m/s^2).
13202 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
13203 * resolved along body-frame axes, averaged over time interval and
13204 * expressed in meters per squared second (m/s^2).
13205 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
13206 * resolved along body-frame axes, averaged over time interval and
13207 * expressed in radians per second (rad/s).
13208 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
13209 * resolved along body-frame axes, averaged over time interval and
13210 * expressed in radians per second (rad/s).
13211 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
13212 * resolved along body-frame axes, averaged over time interval and
13213 * expressed in radians per second (rad/s).
13214 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
13215 * @param result instance where new estimated NED frame containing new body position,
13216 * velocity and coordinate transformation matrix will be stored.
13217 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
13218 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
13219 * body-to-NED-frame coordinate transformation matrix are
13220 * invalid.
13221 */
13222 public static void navigateNED(
13223 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
13224 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
13225 final double fx, final double fy, final double fz,
13226 final double angularRateX, final double angularRateY, final double angularRateZ,
13227 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
13228 InvalidSourceAndDestinationFrameTypeException {
13229
13230 if (!isValidBodyToNEDCoordinateTransformationMatrix(oldC)) {
13231 throw new InvalidSourceAndDestinationFrameTypeException();
13232 }
13233
13234 try {
13235 // Calculate attitude increment, magnitude, and skew-symmetric matrix
13236 final var alphaIbb = new Matrix(ROWS, 1);
13237 alphaIbb.setElementAtIndex(0, angularRateX * timeInterval);
13238 alphaIbb.setElementAtIndex(1, angularRateY * timeInterval);
13239 alphaIbb.setElementAtIndex(2, angularRateZ * timeInterval);
13240
13241 final var magAlpha = Utils.normF(alphaIbb);
13242 final var skewAlpha = Utils.skewMatrix(alphaIbb);
13243
13244 // From (2.123), determine the angular rate of the ECEF frame with respect
13245 // the ECI frame, resolved about NED
13246 final var omegaIen = new Matrix(ROWS, 1);
13247 omegaIen.setElementAtIndex(0, Math.cos(oldLatitude) * EARTH_ROTATION_RATE);
13248 omegaIen.setElementAtIndex(2, -Math.sin(oldLatitude) * EARTH_ROTATION_RATE);
13249
13250 // From (5.44), determine the angular rate of the NED frame with respect
13251 // the ECEF frame, resolved about NED
13252 final var oldRadiiOfCurvature = RadiiOfCurvatureEstimator.estimateRadiiOfCurvatureAndReturnNew(oldLatitude);
13253 final var oldRe = oldRadiiOfCurvature.getRe();
13254 final var oldRn = oldRadiiOfCurvature.getRn();
13255
13256 final var oldRePlusHeight = oldRe + oldHeight;
13257 final var oldOmegaEnN = new Matrix(ROWS, 1);
13258 oldOmegaEnN.setElementAtIndex(0, oldVe / oldRePlusHeight);
13259 oldOmegaEnN.setElementAtIndex(1, -oldVn / (oldRn + oldHeight));
13260 oldOmegaEnN.setElementAtIndex(2, -oldVe * Math.tan(oldLatitude) / oldRePlusHeight);
13261
13262 final var oldCbn = oldC.getMatrix();
13263
13264 final var skewOmega = Utils.skewMatrix(oldOmegaEnN.addAndReturnNew(omegaIen));
13265 skewOmega.multiplyByScalar(0.5);
13266 skewOmega.multiply(oldCbn);
13267
13268 // Calculate the average body-to-ECEF-frame coordinate transformation
13269 // matrix over the update interval using (5.84) and (5.86)
13270 final Matrix aveCbn;
13271 if (magAlpha > ALPHA_THRESHOLD) {
13272 final var magAlpha2 = magAlpha * magAlpha;
13273 final var value1 = (1.0 - Math.cos(magAlpha)) / magAlpha2;
13274 final var value2 = (1.0 - Math.sin(magAlpha) / magAlpha) / magAlpha2;
13275
13276 final var tmp1 = Matrix.identity(ROWS, ROWS);
13277 final var tmp2 = skewAlpha.multiplyByScalarAndReturnNew(value1);
13278 final var tmp3 = skewAlpha.multiplyByScalarAndReturnNew(value2);
13279 tmp3.multiply(skewAlpha);
13280
13281 tmp1.add(tmp2);
13282 tmp1.add(tmp3);
13283
13284 aveCbn = oldCbn.multiplyAndReturnNew(tmp1);
13285 aveCbn.subtract(skewOmega);
13286 } else {
13287 aveCbn = oldCbn.subtractAndReturnNew(skewOmega);
13288 }
13289
13290 // Transform specific force to ECEF-frame resolving axes using (5.86)
13291 final var fIbb = new Matrix(ROWS, 1);
13292 fIbb.setElementAtIndex(0, fx);
13293 fIbb.setElementAtIndex(1, fy);
13294 fIbb.setElementAtIndex(2, fz);
13295
13296 // aveCbn now contains specific force fIbn = aveCbn * fIbb
13297 aveCbn.multiply(fIbb);
13298
13299 // Update velocity
13300 // From (5.54),
13301 final var gravity = NEDGravityEstimator.estimateGravityAndReturnNew(oldLatitude, oldHeight);
13302 final var g = gravity.asMatrix();
13303 aveCbn.add(g);
13304 aveCbn.multiplyByScalar(timeInterval);
13305
13306 final var oldVebn = new Matrix(ROWS, 1);
13307 oldVebn.setElementAtIndex(0, oldVn);
13308 oldVebn.setElementAtIndex(1, oldVe);
13309 oldVebn.setElementAtIndex(2, oldVd);
13310
13311 final var skewOmega2 = Utils.skewMatrix(oldOmegaEnN.addAndReturnNew(
13312 omegaIen.multiplyByScalarAndReturnNew(2.0)));
13313 skewOmega2.multiply(oldVebn);
13314 skewOmega2.multiplyByScalar(timeInterval);
13315
13316 final var vEbn = oldVebn.addAndReturnNew(aveCbn);
13317 vEbn.subtract(skewOmega2);
13318
13319 final var vn = vEbn.getElementAtIndex(0);
13320 final var ve = vEbn.getElementAtIndex(1);
13321 final var vd = vEbn.getElementAtIndex(2);
13322
13323 // Update curvilinear position
13324 // Update height using (5.56)
13325 final var height = oldHeight - 0.5 * timeInterval * (oldVd + vd);
13326
13327 // Update latitude using (5.56)
13328 final var latitude = oldLatitude
13329 + 0.5 * timeInterval * (oldVn / (oldRn + oldHeight) + vn / (oldRn + height));
13330
13331 // Calculate meridian and transverse radii of curvature
13332 final var radiiOfCurvature = RadiiOfCurvatureEstimator.estimateRadiiOfCurvatureAndReturnNew(latitude);
13333 final var rn = radiiOfCurvature.getRn();
13334 final var re = radiiOfCurvature.getRe();
13335
13336 // Update longitude using (5.56)
13337 final var longitude = oldLongitude
13338 + 0.5 * timeInterval * (oldVe / ((oldRe + oldHeight) * Math.cos(oldLatitude))
13339 + ve / ((re + height) * Math.cos(latitude)));
13340
13341 // Attitude update
13342 // From (5.44), determine the angular rate of the NED frame with respect the
13343 // ECEF frame, resolved about NED
13344 final var rePlusHeight = re + height;
13345 final var omegaEnN = new Matrix(ROWS, 1);
13346 omegaEnN.setElementAtIndex(0, ve / rePlusHeight);
13347 omegaEnN.setElementAtIndex(1, -vn / (rn + height));
13348 omegaEnN.setElementAtIndex(2, -ve * Math.tan(latitude) / rePlusHeight);
13349
13350 // Obtain coordinate transformation matrix from the new attitude with respect
13351 // an inertial frame to the old using Rodrigues' formula, (5.73)
13352 final var cNewOld = Matrix.identity(ROWS, ROWS);
13353 if (magAlpha > ALPHA_THRESHOLD) {
13354 final var magAlpha2 = magAlpha * magAlpha;
13355 final var value1 = Math.sin(magAlpha) / magAlpha;
13356 final var value2 = (1.0 - Math.cos(magAlpha)) / magAlpha2;
13357
13358 final var tmp1 = skewAlpha.multiplyByScalarAndReturnNew(value1);
13359 final var tmp2 = skewAlpha.multiplyByScalarAndReturnNew(value2);
13360 tmp2.multiply(skewAlpha);
13361
13362 cNewOld.add(tmp1);
13363 cNewOld.add(tmp2);
13364 } else {
13365 cNewOld.add(skewAlpha);
13366 }
13367
13368 // Update attitude using (5.77)
13369 omegaEnN.multiplyByScalar(0.5);
13370 oldOmegaEnN.multiplyByScalar(0.5);
13371 omegaIen.add(omegaEnN);
13372 omegaIen.add(oldOmegaEnN);
13373
13374 final var skewOmega3 = Utils.skewMatrix(omegaIen);
13375 skewOmega3.multiplyByScalar(timeInterval);
13376
13377 final var cbn = Matrix.identity(ROWS, ROWS);
13378 cbn.subtract(skewOmega3);
13379 cbn.multiply(oldCbn);
13380 cbn.multiply(cNewOld);
13381
13382 result.setPosition(latitude, longitude, height);
13383 result.setVelocityCoordinates(vn, ve, vd);
13384
13385 final var c = new CoordinateTransformation(cbn, FrameType.BODY_FRAME, FrameType.LOCAL_NAVIGATION_FRAME,
13386 accuracyThreshold);
13387 result.setCoordinateTransformation(c);
13388
13389 } catch (final AlgebraException | InvalidRotationMatrixException e) {
13390 throw new InertialNavigatorException(e);
13391 }
13392 }
13393
13394 /**
13395 * Runs precision local-navigation-frame inertial navigation equations.
13396 * NOTE: only the attitude update and specific force frame transformation
13397 * phases are precise.
13398 *
13399 * @param timeInterval time interval between epochs.
13400 * @param oldLatitude previous latitude expressed in radians (rad).
13401 * @param oldLongitude previous longitude expressed in radians (rad).
13402 * @param oldHeight previous height expressed in meters (m).
13403 * @param oldC previous body-to-NED coordinate transformation.
13404 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
13405 * resolved along NED-frame axes and expressed in meters per second (m/s).
13406 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
13407 * resolved along NED-frame axes and expressed in meters per second (m/s).
13408 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
13409 * resolved along NED-frame axes and expressed in meters per second (m/s).
13410 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
13411 * resolved along body-frame axes, averaged over time interval and
13412 * expressed in meters per squared second (m/s^2).
13413 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
13414 * resolved along body-frame axes, averaged over time interval and
13415 * expressed in meters per squared second (m/s^2).
13416 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
13417 * resolved along body-frame axes, averaged over time interval and
13418 * expressed in meters per squared second (m/s^2).
13419 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
13420 * resolved along body-frame axes, averaged over time interval and
13421 * expressed in radians per second (rad/s).
13422 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
13423 * resolved along body-frame axes, averaged over time interval and
13424 * expressed in radians per second (rad/s).
13425 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
13426 * resolved along body-frame axes, averaged over time interval and
13427 * expressed in radians per second (rad/s).
13428 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
13429 * @param result instance where new estimated NED frame containing new body position,
13430 * velocity and coordinate transformation matrix will be stored.
13431 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
13432 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
13433 * body-to-NED-frame coordinate transformation matrix are
13434 * invalid.
13435 */
13436 public static void navigateNED(
13437 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
13438 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
13439 final double fx, final double fy, final double fz,
13440 final double angularRateX, final double angularRateY, final double angularRateZ,
13441 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
13442 InvalidSourceAndDestinationFrameTypeException {
13443 navigateNED(convertTimeToDouble(timeInterval), oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
13444 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
13445 }
13446
13447 /**
13448 * Runs precision local-navigation-frame inertial navigation equations.
13449 * NOTE: only the attitude update and specific force frame transformation
13450 * phases are precise.
13451 *
13452 * @param timeInterval time interval between epochs expressed in seconds (s).
13453 * @param oldPosition previous curvilinear position expressed in terms of latitude,
13454 * longitude and height.
13455 * @param oldC previous body-to-NED coordinate transformation.
13456 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
13457 * resolved along NED-frame axes and expressed in meters per second (m/s).
13458 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
13459 * resolved along NED-frame axes and expressed in meters per second (m/s).
13460 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
13461 * resolved along NED-frame axes and expressed in meters per second (m/s).
13462 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
13463 * resolved along body-frame axes, averaged over time interval and
13464 * expressed in meters per squared second (m/s^2).
13465 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
13466 * resolved along body-frame axes, averaged over time interval and
13467 * expressed in meters per squared second (m/s^2).
13468 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
13469 * resolved along body-frame axes, averaged over time interval and
13470 * expressed in meters per squared second (m/s^2).
13471 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
13472 * resolved along body-frame axes, averaged over time interval and
13473 * expressed in radians per second (rad/s).
13474 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
13475 * resolved along body-frame axes, averaged over time interval and
13476 * expressed in radians per second (rad/s).
13477 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
13478 * resolved along body-frame axes, averaged over time interval and
13479 * expressed in radians per second (rad/s).
13480 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
13481 * @param result instance where new estimated NED frame containing new body position,
13482 * velocity and coordinate transformation matrix will be stored.
13483 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
13484 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
13485 * body-to-NED-frame coordinate transformation matrix are
13486 * invalid.
13487 */
13488 public static void navigateNED(
13489 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
13490 final double oldVn, final double oldVe, final double oldVd,
13491 final double fx, final double fy, final double fz,
13492 final double angularRateX, final double angularRateY, final double angularRateZ,
13493 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
13494 InvalidSourceAndDestinationFrameTypeException {
13495 navigateNED(timeInterval, oldPosition.getLatitude(), oldPosition.getLongitude(),
13496 oldPosition.getHeight(), oldC, oldVn, oldVe, oldVd, fx, fy, fz,
13497 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
13498 }
13499
13500 /**
13501 * Runs precision local-navigation-frame inertial navigation equations.
13502 * NOTE: only the attitude update and specific force frame transformation
13503 * phases are precise.
13504 *
13505 * @param timeInterval time interval between epochs.
13506 * @param oldPosition previous curvilinear position expressed in terms of latitude,
13507 * longitude and height.
13508 * @param oldC previous body-to-NED coordinate transformation.
13509 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
13510 * resolved along NED-frame axes and expressed in meters per second (m/s).
13511 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
13512 * resolved along NED-frame axes and expressed in meters per second (m/s).
13513 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
13514 * resolved along NED-frame axes and expressed in meters per second (m/s).
13515 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
13516 * resolved along body-frame axes, averaged over time interval and
13517 * expressed in meters per squared second (m/s^2).
13518 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
13519 * resolved along body-frame axes, averaged over time interval and
13520 * expressed in meters per squared second (m/s^2).
13521 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
13522 * resolved along body-frame axes, averaged over time interval and
13523 * expressed in meters per squared second (m/s^2).
13524 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
13525 * resolved along body-frame axes, averaged over time interval and
13526 * expressed in radians per second (rad/s).
13527 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
13528 * resolved along body-frame axes, averaged over time interval and
13529 * expressed in radians per second (rad/s).
13530 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
13531 * resolved along body-frame axes, averaged over time interval and
13532 * expressed in radians per second (rad/s).
13533 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
13534 * @param result instance where new estimated NED frame containing new body position,
13535 * velocity and coordinate transformation matrix will be stored.
13536 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
13537 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
13538 * body-to-NED-frame coordinate transformation matrix are
13539 * invalid.
13540 */
13541 public static void navigateNED(
13542 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
13543 final double oldVn, final double oldVe, final double oldVd,
13544 final double fx, final double fy, final double fz,
13545 final double angularRateX, final double angularRateY, final double angularRateZ,
13546 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
13547 InvalidSourceAndDestinationFrameTypeException {
13548 navigateNED(convertTimeToDouble(timeInterval), oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
13549 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
13550 }
13551
13552 /**
13553 * Runs precision local-navigation-frame inertial navigation equations.
13554 * NOTE: only the attitude update and specific force frame transformation
13555 * phases are precise.
13556 *
13557 * @param timeInterval time interval between epochs expressed in seconds (s).
13558 * @param oldLatitude previous latitude expressed in radians (rad).
13559 * @param oldLongitude previous longitude expressed in radians (rad).
13560 * @param oldHeight previous height expressed in meters (m).
13561 * @param oldC previous body-to-NED coordinate transformation.
13562 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
13563 * along north, east and down axes.
13564 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
13565 * resolved along body-frame axes, averaged over time interval and
13566 * expressed in meters per squared second (m/s^2).
13567 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
13568 * resolved along body-frame axes, averaged over time interval and
13569 * expressed in meters per squared second (m/s^2).
13570 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
13571 * resolved along body-frame axes, averaged over time interval and
13572 * expressed in meters per squared second (m/s^2).
13573 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
13574 * resolved along body-frame axes, averaged over time interval and
13575 * expressed in radians per second (rad/s).
13576 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
13577 * resolved along body-frame axes, averaged over time interval and
13578 * expressed in radians per second (rad/s).
13579 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
13580 * resolved along body-frame axes, averaged over time interval and
13581 * expressed in radians per second (rad/s).
13582 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
13583 * @param result instance where new estimated NED frame containing new body position,
13584 * velocity and coordinate transformation matrix will be stored.
13585 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
13586 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
13587 * body-to-NED-frame coordinate transformation matrix are
13588 * invalid.
13589 */
13590 public static void navigateNED(
13591 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
13592 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
13593 final double fx, final double fy, final double fz,
13594 final double angularRateX, final double angularRateY, final double angularRateZ,
13595 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
13596 InvalidSourceAndDestinationFrameTypeException {
13597 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
13598 oldVelocity.getVn(), oldVelocity.getVe(), oldVelocity.getVd(), fx, fy, fz,
13599 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
13600 }
13601
13602 /**
13603 * Runs precision local-navigation-frame inertial navigation equations.
13604 * NOTE: only the attitude update and specific force frame transformation
13605 * phases are precise.
13606 *
13607 * @param timeInterval time interval between epochs.
13608 * @param oldLatitude previous latitude expressed in radians (rad).
13609 * @param oldLongitude previous longitude expressed in radians (rad).
13610 * @param oldHeight previous height expressed in meters (m).
13611 * @param oldC previous body-to-NED coordinate transformation.
13612 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
13613 * along north, east and down axes.
13614 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
13615 * resolved along body-frame axes, averaged over time interval and
13616 * expressed in meters per squared second (m/s^2).
13617 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
13618 * resolved along body-frame axes, averaged over time interval and
13619 * expressed in meters per squared second (m/s^2).
13620 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
13621 * resolved along body-frame axes, averaged over time interval and
13622 * expressed in meters per squared second (m/s^2).
13623 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
13624 * resolved along body-frame axes, averaged over time interval and
13625 * expressed in radians per second (rad/s).
13626 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
13627 * resolved along body-frame axes, averaged over time interval and
13628 * expressed in radians per second (rad/s).
13629 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
13630 * resolved along body-frame axes, averaged over time interval and
13631 * expressed in radians per second (rad/s).
13632 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
13633 * @param result instance where new estimated NED frame containing new body position,
13634 * velocity and coordinate transformation matrix will be stored.
13635 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
13636 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
13637 * body-to-NED-frame coordinate transformation matrix are
13638 * invalid.
13639 */
13640 public static void navigateNED(
13641 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
13642 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
13643 final double fx, final double fy, final double fz,
13644 final double angularRateX, final double angularRateY, final double angularRateZ,
13645 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
13646 InvalidSourceAndDestinationFrameTypeException {
13647 navigateNED(convertTimeToDouble(timeInterval), oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity,
13648 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
13649 }
13650
13651 /**
13652 * Runs precision local-navigation-frame inertial navigation equations.
13653 * NOTE: only the attitude update and specific force frame transformation
13654 * phases are precise.
13655 *
13656 * @param timeInterval time interval between epochs expressed in seconds (s).
13657 * @param oldPosition previous curvilinear position expressed in terms of latitude,
13658 * longitude and height.
13659 * @param oldC previous body-to-NED coordinate transformation.
13660 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
13661 * along north, east and down axes.
13662 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
13663 * resolved along body-frame axes, averaged over time interval and
13664 * expressed in meters per squared second (m/s^2).
13665 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
13666 * resolved along body-frame axes, averaged over time interval and
13667 * expressed in meters per squared second (m/s^2).
13668 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
13669 * resolved along body-frame axes, averaged over time interval and
13670 * expressed in meters per squared second (m/s^2).
13671 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
13672 * resolved along body-frame axes, averaged over time interval and
13673 * expressed in radians per second (rad/s).
13674 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
13675 * resolved along body-frame axes, averaged over time interval and
13676 * expressed in radians per second (rad/s).
13677 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
13678 * resolved along body-frame axes, averaged over time interval and
13679 * expressed in radians per second (rad/s).
13680 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
13681 * @param result instance where new estimated NED frame containing new body position,
13682 * velocity and coordinate transformation matrix will be stored.
13683 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
13684 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
13685 * body-to-NED-frame coordinate transformation matrix are
13686 * invalid.
13687 */
13688 public static void navigateNED(
13689 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
13690 final NEDVelocity oldVelocity, final double fx, final double fy, final double fz,
13691 final double angularRateX, final double angularRateY, final double angularRateZ,
13692 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
13693 InvalidSourceAndDestinationFrameTypeException {
13694 navigateNED(timeInterval, oldPosition.getLatitude(), oldPosition.getLongitude(), oldPosition.getHeight(), oldC,
13695 oldVelocity.getVn(), oldVelocity.getVe(), oldVelocity.getVd(), fx, fy, fz,
13696 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
13697 }
13698
13699 /**
13700 * Runs precision local-navigation-frame inertial navigation equations.
13701 * NOTE: only the attitude update and specific force frame transformation
13702 * phases are precise.
13703 *
13704 * @param timeInterval time interval between epochs.
13705 * @param oldPosition previous curvilinear position expressed in terms of latitude,
13706 * longitude and height.
13707 * @param oldC previous body-to-NED coordinate transformation.
13708 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
13709 * along north, east and down axes.
13710 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
13711 * resolved along body-frame axes, averaged over time interval and
13712 * expressed in meters per squared second (m/s^2).
13713 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
13714 * resolved along body-frame axes, averaged over time interval and
13715 * expressed in meters per squared second (m/s^2).
13716 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
13717 * resolved along body-frame axes, averaged over time interval and
13718 * expressed in meters per squared second (m/s^2).
13719 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
13720 * resolved along body-frame axes, averaged over time interval and
13721 * expressed in radians per second (rad/s).
13722 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
13723 * resolved along body-frame axes, averaged over time interval and
13724 * expressed in radians per second (rad/s).
13725 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
13726 * resolved along body-frame axes, averaged over time interval and
13727 * expressed in radians per second (rad/s).
13728 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
13729 * @param result instance where new estimated NED frame containing new body position,
13730 * velocity and coordinate transformation matrix will be stored.
13731 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
13732 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
13733 * body-to-NED-frame coordinate transformation matrix are
13734 * invalid.
13735 */
13736 public static void navigateNED(
13737 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
13738 final NEDVelocity oldVelocity, final double fx, final double fy, final double fz,
13739 final double angularRateX, final double angularRateY, final double angularRateZ,
13740 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
13741 InvalidSourceAndDestinationFrameTypeException {
13742 navigateNED(convertTimeToDouble(timeInterval), oldPosition, oldC, oldVelocity, fx, fy, fz,
13743 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
13744 }
13745
13746 /**
13747 * Runs precision local-navigation-frame inertial navigation equations.
13748 * NOTE: only the attitude update and specific force frame transformation
13749 * phases are precise.
13750 *
13751 * @param timeInterval time interval between epochs expressed in seconds (s).
13752 * @param oldLatitude previous latitude expressed in radians (rad).
13753 * @param oldLongitude previous longitude expressed in radians (rad).
13754 * @param oldHeight previous height expressed in meters (m).
13755 * @param oldC previous body-to-NED coordinate transformation.
13756 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
13757 * resolved along NED-frame axes and expressed in meters per second (m/s).
13758 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
13759 * resolved along NED-frame axes and expressed in meters per second (m/s).
13760 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
13761 * resolved along NED-frame axes and expressed in meters per second (m/s).
13762 * @param kinematics body kinematics containing specific forces and angular rates applied to
13763 * the body.
13764 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
13765 * @param result instance where new estimated NED frame containing new body position,
13766 * velocity and coordinate transformation matrix will be stored.
13767 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
13768 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
13769 * body-to-NED-frame coordinate transformation matrix are
13770 * invalid.
13771 */
13772 public static void navigateNED(
13773 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
13774 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
13775 final BodyKinematics kinematics, final double accuracyThreshold, final NEDFrame result)
13776 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
13777
13778 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
13779 kinematics.getFx(), kinematics.getFy(), kinematics.getFz(),
13780 kinematics.getAngularRateX(), kinematics.getAngularRateY(), kinematics.getAngularRateZ(),
13781 accuracyThreshold, result);
13782 }
13783
13784 /**
13785 * Runs precision local-navigation-frame inertial navigation equations.
13786 * NOTE: only the attitude update and specific force frame transformation
13787 * phases are precise.
13788 *
13789 * @param timeInterval time interval between epochs expressed in seconds (s).
13790 * @param oldLatitude previous latitude expressed in radians (rad).
13791 * @param oldLongitude previous longitude expressed in radians (rad).
13792 * @param oldHeight previous height expressed in meters (m).
13793 * @param oldC previous body-to-NED coordinate transformation.
13794 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
13795 * resolved along NED-frame axes and expressed in meters per second (m/s).
13796 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
13797 * resolved along NED-frame axes and expressed in meters per second (m/s).
13798 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
13799 * resolved along NED-frame axes and expressed in meters per second (m/s).
13800 * @param kinematics body kinematics containing specific forces and angular rates applied to
13801 * the body.
13802 * @param result instance where new estimated NED frame containing new body position,
13803 * velocity and coordinate transformation matrix will be stored.
13804 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
13805 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
13806 * body-to-NED-frame coordinate transformation matrix are
13807 * invalid.
13808 */
13809 public static void navigateNED(
13810 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
13811 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
13812 final BodyKinematics kinematics, final NEDFrame result) throws InertialNavigatorException,
13813 InvalidSourceAndDestinationFrameTypeException {
13814
13815 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, kinematics,
13816 DEFAULT_ACCURACY_THRESHOLD, result);
13817 }
13818
13819 /**
13820 * Runs precision local-navigation-frame inertial navigation equations.
13821 * NOTE: only the attitude update and specific force frame transformation
13822 * phases are precise.
13823 *
13824 * @param timeInterval time interval between epochs.
13825 * @param oldLatitude previous latitude expressed in radians (rad).
13826 * @param oldLongitude previous longitude expressed in radians (rad).
13827 * @param oldHeight previous height expressed in meters (m).
13828 * @param oldC previous body-to-NED coordinate transformation.
13829 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
13830 * resolved along NED-frame axes and expressed in meters per second (m/s).
13831 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
13832 * resolved along NED-frame axes and expressed in meters per second (m/s).
13833 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
13834 * resolved along NED-frame axes and expressed in meters per second (m/s).
13835 * @param kinematics body kinematics containing specific forces and angular rates applied to
13836 * the body.
13837 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
13838 * @param result instance where new estimated NED frame containing new body position,
13839 * velocity and coordinate transformation matrix will be stored.
13840 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
13841 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
13842 * body-to-NED-frame coordinate transformation matrix are
13843 * invalid.
13844 */
13845 public static void navigateNED(
13846 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
13847 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
13848 final BodyKinematics kinematics, final double accuracyThreshold, final NEDFrame result)
13849 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
13850 navigateNED(convertTimeToDouble(timeInterval), oldLatitude, oldLongitude, oldHeight, oldC,
13851 oldVn, oldVe, oldVd, kinematics, accuracyThreshold, result);
13852 }
13853
13854 /**
13855 * Runs precision local-navigation-frame inertial navigation equations.
13856 * NOTE: only the attitude update and specific force frame transformation
13857 * phases are precise.
13858 *
13859 * @param timeInterval time interval between epochs.
13860 * @param oldLatitude previous latitude expressed in radians (rad).
13861 * @param oldLongitude previous longitude expressed in radians (rad).
13862 * @param oldHeight previous height expressed in meters (m).
13863 * @param oldC previous body-to-NED coordinate transformation.
13864 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
13865 * resolved along NED-frame axes and expressed in meters per second (m/s).
13866 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
13867 * resolved along NED-frame axes and expressed in meters per second (m/s).
13868 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
13869 * resolved along NED-frame axes and expressed in meters per second (m/s).
13870 * @param kinematics body kinematics containing specific forces and angular rates applied to
13871 * the body.
13872 * @param result instance where new estimated NED frame containing new body position,
13873 * velocity and coordinate transformation matrix will be stored.
13874 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
13875 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
13876 * body-to-NED-frame coordinate transformation matrix are
13877 * invalid.
13878 */
13879 public static void navigateNED(
13880 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
13881 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
13882 final BodyKinematics kinematics, final NEDFrame result) throws InertialNavigatorException,
13883 InvalidSourceAndDestinationFrameTypeException {
13884 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, kinematics,
13885 DEFAULT_ACCURACY_THRESHOLD, result);
13886 }
13887
13888 /**
13889 * Runs precision local-navigation-frame inertial navigation equations.
13890 * NOTE: only the attitude update and specific force frame transformation
13891 * phases are precise.
13892 *
13893 * @param timeInterval time interval between epochs expressed in seconds (s).
13894 * @param oldPosition previous curvilinear position expressed in terms of latitude,
13895 * longitude and height.
13896 * @param oldC previous body-to-NED coordinate transformation.
13897 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
13898 * resolved along NED-frame axes and expressed in meters per second (m/s).
13899 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
13900 * resolved along NED-frame axes and expressed in meters per second (m/s).
13901 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
13902 * resolved along NED-frame axes and expressed in meters per second (m/s).
13903 * @param kinematics body kinematics containing specific forces and angular rates applied to
13904 * the body.
13905 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
13906 * @param result instance where new estimated NED frame containing new body position,
13907 * velocity and coordinate transformation matrix will be stored.
13908 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
13909 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
13910 * body-to-NED-frame coordinate transformation matrix are
13911 * invalid.
13912 */
13913 public static void navigateNED(
13914 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
13915 final double oldVn, final double oldVe, final double oldVd, final BodyKinematics kinematics,
13916 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
13917 InvalidSourceAndDestinationFrameTypeException {
13918 navigateNED(timeInterval, oldPosition.getLatitude(), oldPosition.getLongitude(), oldPosition.getHeight(),
13919 oldC, oldVn, oldVe, oldVd, kinematics, accuracyThreshold, result);
13920 }
13921
13922 /**
13923 * Runs precision local-navigation-frame inertial navigation equations.
13924 * NOTE: only the attitude update and specific force frame transformation
13925 * phases are precise.
13926 *
13927 * @param timeInterval time interval between epochs expressed in seconds (s).
13928 * @param oldPosition previous curvilinear position expressed in terms of latitude,
13929 * longitude and height.
13930 * @param oldC previous body-to-NED coordinate transformation.
13931 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
13932 * resolved along NED-frame axes and expressed in meters per second (m/s).
13933 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
13934 * resolved along NED-frame axes and expressed in meters per second (m/s).
13935 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
13936 * resolved along NED-frame axes and expressed in meters per second (m/s).
13937 * @param kinematics body kinematics containing specific forces and angular rates applied to
13938 * the body.
13939 * @param result instance where new estimated NED frame containing new body position,
13940 * velocity and coordinate transformation matrix will be stored.
13941 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
13942 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
13943 * body-to-NED-frame coordinate transformation matrix are
13944 * invalid.
13945 */
13946 public static void navigateNED(
13947 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
13948 final double oldVn, final double oldVe, final double oldVd, final BodyKinematics kinematics,
13949 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
13950 navigateNED(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, kinematics, DEFAULT_ACCURACY_THRESHOLD,
13951 result);
13952 }
13953
13954 /**
13955 * Runs precision local-navigation-frame inertial navigation equations.
13956 * NOTE: only the attitude update and specific force frame transformation
13957 * phases are precise.
13958 *
13959 * @param timeInterval time interval between epochs.
13960 * @param oldPosition previous curvilinear position expressed in terms of latitude,
13961 * longitude and height.
13962 * @param oldC previous body-to-NED coordinate transformation.
13963 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
13964 * resolved along NED-frame axes and expressed in meters per second (m/s).
13965 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
13966 * resolved along NED-frame axes and expressed in meters per second (m/s).
13967 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
13968 * resolved along NED-frame axes and expressed in meters per second (m/s).
13969 * @param kinematics body kinematics containing specific forces and angular rates applied to
13970 * the body.
13971 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
13972 * @param result instance where new estimated NED frame containing new body position,
13973 * velocity and coordinate transformation matrix will be stored.
13974 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
13975 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
13976 * body-to-NED-frame coordinate transformation matrix are
13977 * invalid.
13978 */
13979 public static void navigateNED(
13980 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
13981 final double oldVn, final double oldVe, final double oldVd, final BodyKinematics kinematics,
13982 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
13983 InvalidSourceAndDestinationFrameTypeException {
13984 navigateNED(convertTimeToDouble(timeInterval), oldPosition, oldC, oldVn, oldVe, oldVd, kinematics,
13985 accuracyThreshold, result);
13986 }
13987
13988 /**
13989 * Runs precision local-navigation-frame inertial navigation equations.
13990 * NOTE: only the attitude update and specific force frame transformation
13991 * phases are precise.
13992 *
13993 * @param timeInterval time interval between epochs.
13994 * @param oldPosition previous curvilinear position expressed in terms of latitude,
13995 * longitude and height.
13996 * @param oldC previous body-to-NED coordinate transformation.
13997 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
13998 * resolved along NED-frame axes and expressed in meters per second (m/s).
13999 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
14000 * resolved along NED-frame axes and expressed in meters per second (m/s).
14001 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
14002 * resolved along NED-frame axes and expressed in meters per second (m/s).
14003 * @param kinematics body kinematics containing specific forces and angular rates applied to
14004 * the body.
14005 * @param result instance where new estimated NED frame containing new body position,
14006 * velocity and coordinate transformation matrix will be stored.
14007 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
14008 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
14009 * body-to-NED-frame coordinate transformation matrix are
14010 * invalid.
14011 */
14012 public static void navigateNED(
14013 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
14014 final double oldVn, final double oldVe, final double oldVd, final BodyKinematics kinematics,
14015 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
14016 navigateNED(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, kinematics, DEFAULT_ACCURACY_THRESHOLD,
14017 result);
14018 }
14019
14020 /**
14021 * Runs precision local-navigation-frame inertial navigation equations.
14022 * NOTE: only the attitude update and specific force frame transformation
14023 * phases are precise.
14024 *
14025 * @param timeInterval time interval between epochs expressed in seconds (s).
14026 * @param oldLatitude previous latitude expressed in radians (rad).
14027 * @param oldLongitude previous longitude expressed in radians (rad).
14028 * @param oldHeight previous height expressed in meters (m).
14029 * @param oldC previous body-to-NED coordinate transformation.
14030 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
14031 * along north, east and down axes.
14032 * @param kinematics body kinematics containing specific forces and angular rates applied to
14033 * the body.
14034 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
14035 * @param result instance where new estimated NED frame containing new body position,
14036 * velocity and coordinate transformation matrix will be stored.
14037 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
14038 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
14039 * body-to-NED-frame coordinate transformation matrix are
14040 * invalid.
14041 */
14042 public static void navigateNED(
14043 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
14044 final CoordinateTransformation oldC, final NEDVelocity oldVelocity, final BodyKinematics kinematics,
14045 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
14046 InvalidSourceAndDestinationFrameTypeException {
14047 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
14048 oldVelocity.getVn(), oldVelocity.getVe(), oldVelocity.getVd(), kinematics, accuracyThreshold, result);
14049 }
14050
14051 /**
14052 * Runs precision local-navigation-frame inertial navigation equations.
14053 * NOTE: only the attitude update and specific force frame transformation
14054 * phases are precise.
14055 *
14056 * @param timeInterval time interval between epochs expressed in seconds (s).
14057 * @param oldLatitude previous latitude expressed in radians (rad).
14058 * @param oldLongitude previous longitude expressed in radians (rad).
14059 * @param oldHeight previous height expressed in meters (m).
14060 * @param oldC previous body-to-NED coordinate transformation.
14061 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
14062 * along north, east and down axes.
14063 * @param kinematics body kinematics containing specific forces and angular rates applied to
14064 * the body.
14065 * @param result instance where new estimated NED frame containing new body position,
14066 * velocity and coordinate transformation matrix will be stored.
14067 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
14068 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
14069 * body-to-NED-frame coordinate transformation matrix are
14070 * invalid.
14071 */
14072 public static void navigateNED(
14073 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
14074 final CoordinateTransformation oldC, final NEDVelocity oldVelocity, final BodyKinematics kinematics,
14075 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
14076 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, kinematics,
14077 DEFAULT_ACCURACY_THRESHOLD, result);
14078 }
14079
14080 /**
14081 * Runs precision local-navigation-frame inertial navigation equations.
14082 * NOTE: only the attitude update and specific force frame transformation
14083 * phases are precise.
14084 *
14085 * @param timeInterval time interval between epochs.
14086 * @param oldLatitude previous latitude expressed in radians (rad).
14087 * @param oldLongitude previous longitude expressed in radians (rad).
14088 * @param oldHeight previous height expressed in meters (m).
14089 * @param oldC previous body-to-NED coordinate transformation.
14090 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
14091 * along north, east and down axes.
14092 * @param kinematics body kinematics containing specific forces and angular rates applied to
14093 * the body.
14094 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
14095 * @param result instance where new estimated NED frame containing new body position,
14096 * velocity and coordinate transformation matrix will be stored.
14097 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
14098 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
14099 * body-to-NED-frame coordinate transformation matrix are
14100 * invalid.
14101 */
14102 public static void navigateNED(
14103 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
14104 final CoordinateTransformation oldC, final NEDVelocity oldVelocity, final BodyKinematics kinematics,
14105 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
14106 InvalidSourceAndDestinationFrameTypeException {
14107 navigateNED(convertTimeToDouble(timeInterval), oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity,
14108 kinematics, accuracyThreshold, result);
14109 }
14110
14111 /**
14112 * Runs precision local-navigation-frame inertial navigation equations.
14113 * NOTE: only the attitude update and specific force frame transformation
14114 * phases are precise.
14115 *
14116 * @param timeInterval time interval between epochs.
14117 * @param oldLatitude previous latitude expressed in radians (rad).
14118 * @param oldLongitude previous longitude expressed in radians (rad).
14119 * @param oldHeight previous height expressed in meters (m).
14120 * @param oldC previous body-to-NED coordinate transformation.
14121 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
14122 * along north, east and down axes.
14123 * @param kinematics body kinematics containing specific forces and angular rates applied to
14124 * the body.
14125 * @param result instance where new estimated NED frame containing new body position,
14126 * velocity and coordinate transformation matrix will be stored.
14127 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
14128 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
14129 * body-to-NED-frame coordinate transformation matrix are
14130 * invalid.
14131 */
14132 public static void navigateNED(
14133 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
14134 final CoordinateTransformation oldC, final NEDVelocity oldVelocity, final BodyKinematics kinematics,
14135 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
14136 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, kinematics,
14137 DEFAULT_ACCURACY_THRESHOLD, result);
14138 }
14139
14140 /**
14141 * Runs precision local-navigation-frame inertial navigation equations.
14142 * NOTE: only the attitude update and specific force frame transformation
14143 * phases are precise.
14144 *
14145 * @param timeInterval time interval between epochs expressed in seconds (s).
14146 * @param oldPosition previous curvilinear position expressed in terms of latitude,
14147 * longitude and height.
14148 * @param oldC previous body-to-NED coordinate transformation.
14149 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
14150 * along north, east and down axes.
14151 * @param kinematics body kinematics containing specific forces and angular rates applied to
14152 * the body.
14153 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
14154 * @param result instance where new estimated NED frame containing new body position,
14155 * velocity and coordinate transformation matrix will be stored.
14156 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
14157 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
14158 * body-to-NED-frame coordinate transformation matrix are
14159 * invalid.
14160 */
14161 public static void navigateNED(
14162 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
14163 final NEDVelocity oldVelocity, final BodyKinematics kinematics, final double accuracyThreshold,
14164 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
14165 navigateNED(timeInterval, oldPosition, oldC, oldVelocity.getVn(), oldVelocity.getVe(), oldVelocity.getVd(),
14166 kinematics, accuracyThreshold, result);
14167 }
14168
14169 /**
14170 * Runs precision local-navigation-frame inertial navigation equations.
14171 * NOTE: only the attitude update and specific force frame transformation
14172 * phases are precise.
14173 *
14174 * @param timeInterval time interval between epochs expressed in seconds (s).
14175 * @param oldPosition previous curvilinear position expressed in terms of latitude,
14176 * longitude and height.
14177 * @param oldC previous body-to-NED coordinate transformation.
14178 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
14179 * along north, east and down axes.
14180 * @param kinematics body kinematics containing specific forces and angular rates applied to
14181 * the body.
14182 * @param result instance where new estimated NED frame containing new body position,
14183 * velocity and coordinate transformation matrix will be stored.
14184 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
14185 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
14186 * body-to-NED-frame coordinate transformation matrix are
14187 * invalid.
14188 */
14189 public static void navigateNED(
14190 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
14191 final NEDVelocity oldVelocity, final BodyKinematics kinematics, final NEDFrame result)
14192 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
14193 navigateNED(timeInterval, oldPosition, oldC, oldVelocity, kinematics, DEFAULT_ACCURACY_THRESHOLD, result);
14194 }
14195
14196 /**
14197 * Runs precision local-navigation-frame inertial navigation equations.
14198 * NOTE: only the attitude update and specific force frame transformation
14199 * phases are precise.
14200 *
14201 * @param timeInterval time interval between epochs expressed in seconds (s).
14202 * @param oldPosition previous curvilinear position expressed in terms of latitude,
14203 * longitude and height.
14204 * @param oldC previous body-to-NED coordinate transformation.
14205 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
14206 * along north, east and down axes.
14207 * @param kinematics body kinematics containing specific forces and angular rates applied to
14208 * the body.
14209 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
14210 * @param result instance where new estimated NED frame containing new body position,
14211 * velocity and coordinate transformation matrix will be stored.
14212 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
14213 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
14214 * body-to-NED-frame coordinate transformation matrix are
14215 * invalid.
14216 */
14217 public static void navigateNED(
14218 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
14219 final NEDVelocity oldVelocity, final BodyKinematics kinematics, final double accuracyThreshold,
14220 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
14221 navigateNED(convertTimeToDouble(timeInterval), oldPosition, oldC, oldVelocity, kinematics, accuracyThreshold,
14222 result);
14223 }
14224
14225 /**
14226 * Runs precision local-navigation-frame inertial navigation equations.
14227 * NOTE: only the attitude update and specific force frame transformation
14228 * phases are precise.
14229 *
14230 * @param timeInterval time interval between epochs expressed in seconds (s).
14231 * @param oldPosition previous curvilinear position expressed in terms of latitude,
14232 * longitude and height.
14233 * @param oldC previous body-to-NED coordinate transformation.
14234 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
14235 * along north, east and down axes.
14236 * @param kinematics body kinematics containing specific forces and angular rates applied to
14237 * the body.
14238 * @param result instance where new estimated NED frame containing new body position,
14239 * velocity and coordinate transformation matrix will be stored.
14240 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
14241 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
14242 * body-to-NED-frame coordinate transformation matrix are
14243 * invalid.
14244 */
14245 public static void navigateNED(
14246 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
14247 final NEDVelocity oldVelocity, final BodyKinematics kinematics, final NEDFrame result)
14248 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
14249 navigateNED(timeInterval, oldPosition, oldC, oldVelocity, kinematics, DEFAULT_ACCURACY_THRESHOLD, result);
14250 }
14251
14252 /**
14253 * Runs precision local-navigation-frame inertial navigation equations.
14254 * NOTE: only the attitude update and specific force frame transformation
14255 * phases are precise.
14256 *
14257 * @param timeInterval time interval between epochs expressed in seconds (s).
14258 * @param oldLatitude previous latitude angle.
14259 * @param oldLongitude previous longitude angle.
14260 * @param oldHeight previous height.
14261 * @param oldC previous body-to-NED coordinate transformation.
14262 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
14263 * resolved along NED-frame axes and expressed in meters per second (m/s).
14264 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
14265 * resolved along NED-frame axes and expressed in meters per second (m/s).
14266 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
14267 * resolved along NED-frame axes and expressed in meters per second (m/s).
14268 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
14269 * resolved along body-frame axes, averaged over time interval and
14270 * expressed in meters per squared second (m/s^2).
14271 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
14272 * resolved along body-frame axes, averaged over time interval and
14273 * expressed in meters per squared second (m/s^2).
14274 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
14275 * resolved along body-frame axes, averaged over time interval and
14276 * expressed in meters per squared second (m/s^2).
14277 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
14278 * resolved along body-frame axes, averaged over time interval and
14279 * expressed in radians per second (rad/s).
14280 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
14281 * resolved along body-frame axes, averaged over time interval and
14282 * expressed in radians per second (rad/s).
14283 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
14284 * resolved along body-frame axes, averaged over time interval and
14285 * expressed in radians per second (rad/s).
14286 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
14287 * @param result instance where new estimated NED frame containing new body position,
14288 * velocity and coordinate transformation matrix will be stored.
14289 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
14290 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
14291 * body-to-NED-frame coordinate transformation matrix are
14292 * invalid.
14293 */
14294 public static void navigateNED(
14295 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
14296 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
14297 final double fx, final double fy, final double fz, final double angularRateX, final double angularRateY,
14298 final double angularRateZ, final double accuracyThreshold, final NEDFrame result)
14299 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
14300 navigateNED(timeInterval, convertAngleToDouble(oldLatitude), convertAngleToDouble(oldLongitude),
14301 convertDistanceToDouble(oldHeight), oldC, oldVn, oldVe, oldVd, fx, fy, fz,
14302 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
14303 }
14304
14305 /**
14306 * Runs precision local-navigation-frame inertial navigation equations.
14307 * NOTE: only the attitude update and specific force frame transformation
14308 * phases are precise.
14309 *
14310 * @param timeInterval time interval between epochs.
14311 * @param oldLatitude previous latitude angle.
14312 * @param oldLongitude previous longitude angle.
14313 * @param oldHeight previous height.
14314 * @param oldC previous body-to-NED coordinate transformation.
14315 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
14316 * resolved along NED-frame axes and expressed in meters per second (m/s).
14317 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
14318 * resolved along NED-frame axes and expressed in meters per second (m/s).
14319 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
14320 * resolved along NED-frame axes and expressed in meters per second (m/s).
14321 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
14322 * resolved along body-frame axes, averaged over time interval and
14323 * expressed in meters per squared second (m/s^2).
14324 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
14325 * resolved along body-frame axes, averaged over time interval and
14326 * expressed in meters per squared second (m/s^2).
14327 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
14328 * resolved along body-frame axes, averaged over time interval and
14329 * expressed in meters per squared second (m/s^2).
14330 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
14331 * resolved along body-frame axes, averaged over time interval and
14332 * expressed in radians per second (rad/s).
14333 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
14334 * resolved along body-frame axes, averaged over time interval and
14335 * expressed in radians per second (rad/s).
14336 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
14337 * resolved along body-frame axes, averaged over time interval and
14338 * expressed in radians per second (rad/s).
14339 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
14340 * @param result instance where new estimated NED frame containing new body position,
14341 * velocity and coordinate transformation matrix will be stored.
14342 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
14343 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
14344 * body-to-NED-frame coordinate transformation matrix are
14345 * invalid.
14346 */
14347 public static void navigateNED(
14348 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude,
14349 final Distance oldHeight, final CoordinateTransformation oldC,
14350 final double oldVn, final double oldVe, final double oldVd,
14351 final double fx, final double fy, final double fz,
14352 final double angularRateX, final double angularRateY, final double angularRateZ,
14353 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
14354 InvalidSourceAndDestinationFrameTypeException {
14355 navigateNED(convertTimeToDouble(timeInterval), oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
14356 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
14357 }
14358
14359 /**
14360 * Runs precision local-navigation-frame inertial navigation equations.
14361 * NOTE: only the attitude update and specific force frame transformation
14362 * phases are precise.
14363 *
14364 * @param timeInterval time interval between epochs expressed in seconds (s).
14365 * @param oldLatitude previous latitude angle.
14366 * @param oldLongitude previous longitude angle.
14367 * @param oldHeight previous height.
14368 * @param oldC previous body-to-NED coordinate transformation.
14369 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
14370 * along north, east and down axes.
14371 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
14372 * resolved along body-frame axes, averaged over time interval and
14373 * expressed in meters per squared second (m/s^2).
14374 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
14375 * resolved along body-frame axes, averaged over time interval and
14376 * expressed in meters per squared second (m/s^2).
14377 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
14378 * resolved along body-frame axes, averaged over time interval and
14379 * expressed in meters per squared second (m/s^2).
14380 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
14381 * resolved along body-frame axes, averaged over time interval and
14382 * expressed in radians per second (rad/s).
14383 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
14384 * resolved along body-frame axes, averaged over time interval and
14385 * expressed in radians per second (rad/s).
14386 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
14387 * resolved along body-frame axes, averaged over time interval and
14388 * expressed in radians per second (rad/s).
14389 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
14390 * @param result instance where new estimated NED frame containing new body position,
14391 * velocity and coordinate transformation matrix will be stored.
14392 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
14393 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
14394 * body-to-NED-frame coordinate transformation matrix are
14395 * invalid.
14396 */
14397 public static void navigateNED(
14398 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
14399 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
14400 final double fx, final double fy, final double fz,
14401 final double angularRateX, final double angularRateY, final double angularRateZ,
14402 final double accuracyThreshold, final NEDFrame result)
14403 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
14404 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
14405 oldVelocity.getVn(), oldVelocity.getVe(), oldVelocity.getVd(), fx, fy, fz,
14406 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
14407 }
14408
14409 /**
14410 * Runs precision local-navigation-frame inertial navigation equations.
14411 * NOTE: only the attitude update and specific force frame transformation
14412 * phases are precise.
14413 *
14414 * @param timeInterval time interval between epochs.
14415 * @param oldLatitude previous latitude angle.
14416 * @param oldLongitude previous longitude angle.
14417 * @param oldHeight previous height.
14418 * @param oldC previous body-to-NED coordinate transformation.
14419 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
14420 * along north, east and down axes.
14421 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
14422 * resolved along body-frame axes, averaged over time interval and
14423 * expressed in meters per squared second (m/s^2).
14424 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
14425 * resolved along body-frame axes, averaged over time interval and
14426 * expressed in meters per squared second (m/s^2).
14427 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
14428 * resolved along body-frame axes, averaged over time interval and
14429 * expressed in meters per squared second (m/s^2).
14430 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
14431 * resolved along body-frame axes, averaged over time interval and
14432 * expressed in radians per second (rad/s).
14433 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
14434 * resolved along body-frame axes, averaged over time interval and
14435 * expressed in radians per second (rad/s).
14436 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
14437 * resolved along body-frame axes, averaged over time interval and
14438 * expressed in radians per second (rad/s).
14439 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
14440 * @param result instance where new estimated NED frame containing new body position,
14441 * velocity and coordinate transformation matrix will be stored.
14442 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
14443 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
14444 * body-to-NED-frame coordinate transformation matrix are
14445 * invalid.
14446 */
14447 public static void navigateNED(
14448 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
14449 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
14450 final double fx, final double fy, final double fz,
14451 final double angularRateX, final double angularRateY, final double angularRateZ,
14452 final double accuracyThreshold, final NEDFrame result)
14453 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
14454 navigateNED(convertTimeToDouble(timeInterval), oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity,
14455 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
14456 }
14457
14458 /**
14459 * Runs precision local-navigation-frame inertial navigation equations.
14460 * NOTE: only the attitude update and specific force frame transformation
14461 * phases are precise.
14462 *
14463 * @param timeInterval time interval between epochs expressed in seconds (s).
14464 * @param oldLatitude previous latitude angle.
14465 * @param oldLongitude previous longitude angle.
14466 * @param oldHeight previous height.
14467 * @param oldC previous body-to-NED coordinate transformation.
14468 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
14469 * resolved along NED-frame axes and expressed in meters per second (m/s).
14470 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
14471 * resolved along NED-frame axes and expressed in meters per second (m/s).
14472 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
14473 * resolved along NED-frame axes and expressed in meters per second (m/s).
14474 * @param kinematics body kinematics containing specific forces and angular rates applied to
14475 * the body.
14476 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
14477 * @param result instance where new estimated NED frame containing new body position,
14478 * velocity and coordinate transformation matrix will be stored.
14479 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
14480 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
14481 * body-to-NED-frame coordinate transformation matrix are
14482 * invalid.
14483 */
14484 public static void navigateNED(
14485 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
14486 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
14487 final BodyKinematics kinematics, final double accuracyThreshold, final NEDFrame result)
14488 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
14489 navigateNED(timeInterval, convertAngleToDouble(oldLatitude), convertAngleToDouble(oldLongitude),
14490 convertDistanceToDouble(oldHeight), oldC, oldVn, oldVe, oldVd, kinematics, accuracyThreshold, result);
14491 }
14492
14493 /**
14494 * Runs precision local-navigation-frame inertial navigation equations.
14495 * NOTE: only the attitude update and specific force frame transformation
14496 * phases are precise.
14497 *
14498 * @param timeInterval time interval between epochs expressed in seconds (s).
14499 * @param oldLatitude previous latitude angle.
14500 * @param oldLongitude previous longitude angle.
14501 * @param oldHeight previous height.
14502 * @param oldC previous body-to-NED coordinate transformation.
14503 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
14504 * resolved along NED-frame axes and expressed in meters per second (m/s).
14505 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
14506 * resolved along NED-frame axes and expressed in meters per second (m/s).
14507 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
14508 * resolved along NED-frame axes and expressed in meters per second (m/s).
14509 * @param kinematics body kinematics containing specific forces and angular rates applied to
14510 * the body.
14511 * @param result instance where new estimated NED frame containing new body position,
14512 * velocity and coordinate transformation matrix will be stored.
14513 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
14514 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
14515 * body-to-NED-frame coordinate transformation matrix are
14516 * invalid.
14517 */
14518 public static void navigateNED(
14519 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
14520 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
14521 final BodyKinematics kinematics, final NEDFrame result) throws InertialNavigatorException,
14522 InvalidSourceAndDestinationFrameTypeException {
14523 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, kinematics,
14524 DEFAULT_ACCURACY_THRESHOLD, result);
14525 }
14526
14527 /**
14528 * Runs precision local-navigation-frame inertial navigation equations.
14529 * NOTE: only the attitude update and specific force frame transformation
14530 * phases are precise.
14531 *
14532 * @param timeInterval time interval between epochs.
14533 * @param oldLatitude previous latitude angle.
14534 * @param oldLongitude previous longitude angle.
14535 * @param oldHeight previous height.
14536 * @param oldC previous body-to-NED coordinate transformation.
14537 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
14538 * resolved along NED-frame axes and expressed in meters per second (m/s).
14539 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
14540 * resolved along NED-frame axes and expressed in meters per second (m/s).
14541 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
14542 * resolved along NED-frame axes and expressed in meters per second (m/s).
14543 * @param kinematics body kinematics containing specific forces and angular rates applied to
14544 * the body.
14545 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
14546 * @param result instance where new estimated NED frame containing new body position,
14547 * velocity and coordinate transformation matrix will be stored.
14548 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
14549 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
14550 * body-to-NED-frame coordinate transformation matrix are
14551 * invalid.
14552 */
14553 public static void navigateNED(
14554 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
14555 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
14556 final BodyKinematics kinematics, final double accuracyThreshold, final NEDFrame result)
14557 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
14558 navigateNED(convertTimeToDouble(timeInterval), oldLatitude, oldLongitude, oldHeight, oldC,
14559 oldVn, oldVe, oldVd, kinematics, accuracyThreshold, result);
14560 }
14561
14562 /**
14563 * Runs precision local-navigation-frame inertial navigation equations.
14564 * NOTE: only the attitude update and specific force frame transformation
14565 * phases are precise.
14566 *
14567 * @param timeInterval time interval between epochs.
14568 * @param oldLatitude previous latitude angle.
14569 * @param oldLongitude previous longitude angle.
14570 * @param oldHeight previous height.
14571 * @param oldC previous body-to-NED coordinate transformation.
14572 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
14573 * resolved along NED-frame axes and expressed in meters per second (m/s).
14574 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
14575 * resolved along NED-frame axes and expressed in meters per second (m/s).
14576 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
14577 * resolved along NED-frame axes and expressed in meters per second (m/s).
14578 * @param kinematics body kinematics containing specific forces and angular rates applied to
14579 * the body.
14580 * @param result instance where new estimated NED frame containing new body position,
14581 * velocity and coordinate transformation matrix will be stored.
14582 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
14583 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
14584 * body-to-NED-frame coordinate transformation matrix are
14585 * invalid.
14586 */
14587 public static void navigateNED(
14588 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
14589 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
14590 final BodyKinematics kinematics, final NEDFrame result) throws InertialNavigatorException,
14591 InvalidSourceAndDestinationFrameTypeException {
14592 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, kinematics,
14593 DEFAULT_ACCURACY_THRESHOLD, result);
14594 }
14595
14596 /**
14597 * Runs precision local-navigation-frame inertial navigation equations.
14598 * NOTE: only the attitude update and specific force frame transformation
14599 * phases are precise.
14600 *
14601 * @param timeInterval time interval between epochs expressed in seconds (s).
14602 * @param oldLatitude previous latitude angle.
14603 * @param oldLongitude previous longitude angle.
14604 * @param oldHeight previous height.
14605 * @param oldC previous body-to-NED coordinate transformation.
14606 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
14607 * along north, east and down axes.
14608 * @param kinematics body kinematics containing specific forces and angular rates applied to
14609 * the body.
14610 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
14611 * @param result instance where new estimated NED frame containing new body position,
14612 * velocity and coordinate transformation matrix will be stored.
14613 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
14614 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
14615 * body-to-NED-frame coordinate transformation matrix are
14616 * invalid.
14617 */
14618 public static void navigateNED(
14619 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
14620 final CoordinateTransformation oldC, final NEDVelocity oldVelocity, final BodyKinematics kinematics,
14621 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
14622 InvalidSourceAndDestinationFrameTypeException {
14623 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
14624 oldVelocity.getVn(), oldVelocity.getVe(), oldVelocity.getVd(), kinematics, accuracyThreshold, result);
14625 }
14626
14627 /**
14628 * Runs precision local-navigation-frame inertial navigation equations.
14629 * NOTE: only the attitude update and specific force frame transformation
14630 * phases are precise.
14631 *
14632 * @param timeInterval time interval between epochs expressed in seconds (s).
14633 * @param oldLatitude previous latitude angle.
14634 * @param oldLongitude previous longitude angle.
14635 * @param oldHeight previous height.
14636 * @param oldC previous body-to-NED coordinate transformation.
14637 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
14638 * along north, east and down axes.
14639 * @param kinematics body kinematics containing specific forces and angular rates applied to
14640 * the body.
14641 * @param result instance where new estimated NED frame containing new body position,
14642 * velocity and coordinate transformation matrix will be stored.
14643 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
14644 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
14645 * body-to-NED-frame coordinate transformation matrix are
14646 * invalid.
14647 */
14648 public static void navigateNED(
14649 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
14650 final CoordinateTransformation oldC, final NEDVelocity oldVelocity, final BodyKinematics kinematics,
14651 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
14652 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, kinematics,
14653 DEFAULT_ACCURACY_THRESHOLD, result);
14654 }
14655
14656 /**
14657 * Runs precision local-navigation-frame inertial navigation equations.
14658 * NOTE: only the attitude update and specific force frame transformation
14659 * phases are precise.
14660 *
14661 * @param timeInterval time interval between epochs.
14662 * @param oldLatitude previous latitude angle.
14663 * @param oldLongitude previous longitude angle.
14664 * @param oldHeight previous height.
14665 * @param oldC previous body-to-NED coordinate transformation.
14666 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
14667 * along north, east and down axes.
14668 * @param kinematics body kinematics containing specific forces and angular rates applied to
14669 * the body.
14670 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
14671 * @param result instance where new estimated NED frame containing new body position,
14672 * velocity and coordinate transformation matrix will be stored.
14673 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
14674 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
14675 * body-to-NED-frame coordinate transformation matrix are
14676 * invalid.
14677 */
14678 public static void navigateNED(
14679 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
14680 final CoordinateTransformation oldC, final NEDVelocity oldVelocity, final BodyKinematics kinematics,
14681 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
14682 InvalidSourceAndDestinationFrameTypeException {
14683 navigateNED(convertTimeToDouble(timeInterval), oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity,
14684 kinematics, accuracyThreshold, result);
14685 }
14686
14687 /**
14688 * Runs precision local-navigation-frame inertial navigation equations.
14689 * NOTE: only the attitude update and specific force frame transformation
14690 * phases are precise.
14691 *
14692 * @param timeInterval time interval between epochs.
14693 * @param oldLatitude previous latitude angle.
14694 * @param oldLongitude previous longitude angle.
14695 * @param oldHeight previous height.
14696 * @param oldC previous body-to-NED coordinate transformation.
14697 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
14698 * along north, east and down axes.
14699 * @param kinematics body kinematics containing specific forces and angular rates applied to
14700 * the body.
14701 * @param result instance where new estimated NED frame containing new body position,
14702 * velocity and coordinate transformation matrix will be stored.
14703 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
14704 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
14705 * body-to-NED-frame coordinate transformation matrix are
14706 * invalid.
14707 */
14708 public static void navigateNED(
14709 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
14710 final CoordinateTransformation oldC, final NEDVelocity oldVelocity, final BodyKinematics kinematics,
14711 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
14712 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, kinematics,
14713 DEFAULT_ACCURACY_THRESHOLD, result);
14714 }
14715
14716 /**
14717 * Runs precision local-navigation-frame inertial navigation equations.
14718 * NOTE: only the attitude update and specific force frame transformation
14719 * phases are precise.
14720 *
14721 * @param timeInterval time interval between epochs expressed in seconds (s).
14722 * @param oldLatitude previous latitude expressed in radians (rad).
14723 * @param oldLongitude previous longitude expressed in radians (rad).
14724 * @param oldHeight previous height expressed in meters (m).
14725 * @param oldC previous body-to-NED coordinate transformation.
14726 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
14727 * resolved along NED-frame axes.
14728 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
14729 * resolved along NED-frame axes.
14730 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
14731 * resolved along NED-frame axes.
14732 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
14733 * resolved along body-frame axes, averaged over time interval and
14734 * expressed in meters per squared second (m/s^2).
14735 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
14736 * resolved along body-frame axes, averaged over time interval and
14737 * expressed in meters per squared second (m/s^2).
14738 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
14739 * resolved along body-frame axes, averaged over time interval and
14740 * expressed in meters per squared second (m/s^2).
14741 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
14742 * resolved along body-frame axes, averaged over time interval and
14743 * expressed in radians per second (rad/s).
14744 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
14745 * resolved along body-frame axes, averaged over time interval and
14746 * expressed in radians per second (rad/s).
14747 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
14748 * resolved along body-frame axes, averaged over time interval and
14749 * expressed in radians per second (rad/s).
14750 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
14751 * @param result instance where new estimated NED frame containing new body position,
14752 * velocity and coordinate transformation matrix will be stored.
14753 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
14754 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
14755 * body-to-NED-frame coordinate transformation matrix are
14756 * invalid.
14757 */
14758 public static void navigateNED(
14759 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
14760 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
14761 final double fx, final double fy, final double fz,
14762 final double angularRateX, final double angularRateY, final double angularRateZ,
14763 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
14764 InvalidSourceAndDestinationFrameTypeException {
14765 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
14766 convertSpeedToDouble(oldSpeedN), convertSpeedToDouble(oldSpeedE), convertSpeedToDouble(oldSpeedD),
14767 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
14768 }
14769
14770 /**
14771 * Runs precision local-navigation-frame inertial navigation equations.
14772 * NOTE: only the attitude update and specific force frame transformation
14773 * phases are precise.
14774 *
14775 * @param timeInterval time interval between epochs.
14776 * @param oldLatitude previous latitude expressed in radians (rad).
14777 * @param oldLongitude previous longitude expressed in radians (rad).
14778 * @param oldHeight previous height expressed in meters (m).
14779 * @param oldC previous body-to-NED coordinate transformation.
14780 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
14781 * resolved along NED-frame axes.
14782 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
14783 * resolved along NED-frame axes.
14784 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
14785 * resolved along NED-frame axes.
14786 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
14787 * resolved along body-frame axes, averaged over time interval and
14788 * expressed in meters per squared second (m/s^2).
14789 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
14790 * resolved along body-frame axes, averaged over time interval and
14791 * expressed in meters per squared second (m/s^2).
14792 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
14793 * resolved along body-frame axes, averaged over time interval and
14794 * expressed in meters per squared second (m/s^2).
14795 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
14796 * resolved along body-frame axes, averaged over time interval and
14797 * expressed in radians per second (rad/s).
14798 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
14799 * resolved along body-frame axes, averaged over time interval and
14800 * expressed in radians per second (rad/s).
14801 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
14802 * resolved along body-frame axes, averaged over time interval and
14803 * expressed in radians per second (rad/s).
14804 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
14805 * @param result instance where new estimated NED frame containing new body position,
14806 * velocity and coordinate transformation matrix will be stored.
14807 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
14808 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
14809 * body-to-NED-frame coordinate transformation matrix are
14810 * invalid.
14811 */
14812 public static void navigateNED(
14813 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
14814 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
14815 final double fx, final double fy, final double fz,
14816 final double angularRateX, final double angularRateY, final double angularRateZ,
14817 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
14818 InvalidSourceAndDestinationFrameTypeException {
14819 navigateNED(convertTimeToDouble(timeInterval), oldLatitude, oldLongitude, oldHeight, oldC,
14820 oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
14821 accuracyThreshold, result);
14822 }
14823
14824 /**
14825 * Runs precision local-navigation-frame inertial navigation equations.
14826 * NOTE: only the attitude update and specific force frame transformation
14827 * phases are precise.
14828 *
14829 * @param timeInterval time interval between epochs expressed in seconds (s).
14830 * @param oldPosition previous curvilinear position expressed in terms of latitude,
14831 * longitude and height.
14832 * @param oldC previous body-to-NED coordinate transformation.
14833 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
14834 * resolved along NED-frame axes.
14835 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
14836 * resolved along NED-frame axes.
14837 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
14838 * resolved along NED-frame axes.
14839 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
14840 * resolved along body-frame axes, averaged over time interval and
14841 * expressed in meters per squared second (m/s^2).
14842 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
14843 * resolved along body-frame axes, averaged over time interval and
14844 * expressed in meters per squared second (m/s^2).
14845 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
14846 * resolved along body-frame axes, averaged over time interval and
14847 * expressed in meters per squared second (m/s^2).
14848 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
14849 * resolved along body-frame axes, averaged over time interval and
14850 * expressed in radians per second (rad/s).
14851 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
14852 * resolved along body-frame axes, averaged over time interval and
14853 * expressed in radians per second (rad/s).
14854 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
14855 * resolved along body-frame axes, averaged over time interval and
14856 * expressed in radians per second (rad/s).
14857 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
14858 * @param result instance where new estimated NED frame containing new body position,
14859 * velocity and coordinate transformation matrix will be stored.
14860 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
14861 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
14862 * body-to-NED-frame coordinate transformation matrix are
14863 * invalid.
14864 */
14865 public static void navigateNED(
14866 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
14867 final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
14868 final double fx, final double fy, final double fz,
14869 final double angularRateX, final double angularRateY, final double angularRateZ,
14870 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
14871 InvalidSourceAndDestinationFrameTypeException {
14872 navigateNED(timeInterval, oldPosition.getLatitude(), oldPosition.getLongitude(), oldPosition.getHeight(), oldC,
14873 oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
14874 accuracyThreshold, result);
14875 }
14876
14877 /**
14878 * Runs precision local-navigation-frame inertial navigation equations.
14879 * NOTE: only the attitude update and specific force frame transformation
14880 * phases are precise.
14881 *
14882 * @param timeInterval time interval between epochs.
14883 * @param oldPosition previous curvilinear position expressed in terms of latitude,
14884 * longitude and height.
14885 * @param oldC previous body-to-NED coordinate transformation.
14886 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
14887 * resolved along NED-frame axes.
14888 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
14889 * resolved along NED-frame axes.
14890 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
14891 * resolved along NED-frame axes.
14892 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
14893 * resolved along body-frame axes, averaged over time interval and
14894 * expressed in meters per squared second (m/s^2).
14895 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
14896 * resolved along body-frame axes, averaged over time interval and
14897 * expressed in meters per squared second (m/s^2).
14898 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
14899 * resolved along body-frame axes, averaged over time interval and
14900 * expressed in meters per squared second (m/s^2).
14901 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
14902 * resolved along body-frame axes, averaged over time interval and
14903 * expressed in radians per second (rad/s).
14904 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
14905 * resolved along body-frame axes, averaged over time interval and
14906 * expressed in radians per second (rad/s).
14907 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
14908 * resolved along body-frame axes, averaged over time interval and
14909 * expressed in radians per second (rad/s).
14910 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
14911 * @param result instance where new estimated NED frame containing new body position,
14912 * velocity and coordinate transformation matrix will be stored.
14913 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
14914 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
14915 * body-to-NED-frame coordinate transformation matrix are
14916 * invalid.
14917 */
14918 public static void navigateNED(
14919 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
14920 final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
14921 final double fx, final double fy, final double fz,
14922 final double angularRateX, final double angularRateY, final double angularRateZ,
14923 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
14924 InvalidSourceAndDestinationFrameTypeException {
14925 navigateNED(convertTimeToDouble(timeInterval), oldPosition, oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
14926 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
14927 }
14928
14929 /**
14930 * Runs precision local-navigation-frame inertial navigation equations.
14931 * NOTE: only the attitude update and specific force frame transformation
14932 * phases are precise.
14933 *
14934 * @param timeInterval time interval between epochs expressed in seconds (s).
14935 * @param oldLatitude previous latitude expressed in radians (rad).
14936 * @param oldLongitude previous longitude expressed in radians (rad).
14937 * @param oldHeight previous height expressed in meters (m).
14938 * @param oldC previous body-to-NED coordinate transformation.
14939 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
14940 * resolved along NED-frame axes.
14941 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
14942 * resolved along NED-frame axes.
14943 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
14944 * resolved along NED-frame axes.
14945 * @param kinematics body kinematics containing specific forces and angular rates applied to
14946 * the body.
14947 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
14948 * @param result instance where new estimated NED frame containing new body position,
14949 * velocity and coordinate transformation matrix will be stored.
14950 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
14951 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
14952 * body-to-NED-frame coordinate transformation matrix are
14953 * invalid.
14954 */
14955 public static void navigateNED(
14956 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
14957 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
14958 final BodyKinematics kinematics, final double accuracyThreshold, final NEDFrame result)
14959 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
14960 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
14961 kinematics.getFx(), kinematics.getFy(), kinematics.getFz(),
14962 kinematics.getAngularRateX(), kinematics.getAngularRateY(), kinematics.getAngularRateZ(),
14963 accuracyThreshold, result);
14964 }
14965
14966 /**
14967 * Runs precision local-navigation-frame inertial navigation equations.
14968 * NOTE: only the attitude update and specific force frame transformation
14969 * phases are precise.
14970 *
14971 * @param timeInterval time interval between epochs expressed in seconds (s).
14972 * @param oldLatitude previous latitude expressed in radians (rad).
14973 * @param oldLongitude previous longitude expressed in radians (rad).
14974 * @param oldHeight previous height expressed in meters (m).
14975 * @param oldC previous body-to-NED coordinate transformation.
14976 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
14977 * resolved along NED-frame axes.
14978 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
14979 * resolved along NED-frame axes.
14980 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
14981 * resolved along NED-frame axes.
14982 * @param kinematics body kinematics containing specific forces and angular rates applied to
14983 * the body.
14984 * @param result instance where new estimated NED frame containing new body position,
14985 * velocity and coordinate transformation matrix will be stored.
14986 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
14987 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
14988 * body-to-NED-frame coordinate transformation matrix are
14989 * invalid.
14990 */
14991 public static void navigateNED(
14992 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
14993 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
14994 final BodyKinematics kinematics, final NEDFrame result) throws InertialNavigatorException,
14995 InvalidSourceAndDestinationFrameTypeException {
14996 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
14997 kinematics, DEFAULT_ACCURACY_THRESHOLD, result);
14998 }
14999
15000 /**
15001 * Runs precision local-navigation-frame inertial navigation equations.
15002 * NOTE: only the attitude update and specific force frame transformation
15003 * phases are precise.
15004 *
15005 * @param timeInterval time interval between epochs.
15006 * @param oldLatitude previous latitude expressed in radians (rad).
15007 * @param oldLongitude previous longitude expressed in radians (rad).
15008 * @param oldHeight previous height expressed in meters (m).
15009 * @param oldC previous body-to-NED coordinate transformation.
15010 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
15011 * resolved along NED-frame axes.
15012 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
15013 * resolved along NED-frame axes.
15014 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
15015 * resolved along NED-frame axes.
15016 * @param kinematics body kinematics containing specific forces and angular rates applied to
15017 * the body.
15018 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
15019 * @param result instance where new estimated NED frame containing new body position,
15020 * velocity and coordinate transformation matrix will be stored.
15021 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
15022 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
15023 * body-to-NED-frame coordinate transformation matrix are
15024 * invalid.
15025 */
15026 public static void navigateNED(
15027 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
15028 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
15029 final BodyKinematics kinematics, final double accuracyThreshold, final NEDFrame result)
15030 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
15031 navigateNED(convertTimeToDouble(timeInterval), oldLatitude, oldLongitude, oldHeight, oldC,
15032 oldSpeedN, oldSpeedE, oldSpeedD, kinematics, accuracyThreshold, result);
15033 }
15034
15035 /**
15036 * Runs precision local-navigation-frame inertial navigation equations.
15037 * NOTE: only the attitude update and specific force frame transformation
15038 * phases are precise.
15039 *
15040 * @param timeInterval time interval between epochs.
15041 * @param oldLatitude previous latitude expressed in radians (rad).
15042 * @param oldLongitude previous longitude expressed in radians (rad).
15043 * @param oldHeight previous height expressed in meters (m).
15044 * @param oldC previous body-to-NED coordinate transformation.
15045 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
15046 * resolved along NED-frame axes.
15047 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
15048 * resolved along NED-frame axes.
15049 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
15050 * resolved along NED-frame axes.
15051 * @param kinematics body kinematics containing specific forces and angular rates applied to
15052 * the body.
15053 * @param result instance where new estimated NED frame containing new body position,
15054 * velocity and coordinate transformation matrix will be stored.
15055 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
15056 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
15057 * body-to-NED-frame coordinate transformation matrix are
15058 * invalid.
15059 */
15060 public static void navigateNED(
15061 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
15062 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
15063 final BodyKinematics kinematics, final NEDFrame result) throws InertialNavigatorException,
15064 InvalidSourceAndDestinationFrameTypeException {
15065 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
15066 kinematics, DEFAULT_ACCURACY_THRESHOLD, result);
15067 }
15068
15069 /**
15070 * Runs precision local-navigation-frame inertial navigation equations.
15071 * NOTE: only the attitude update and specific force frame transformation
15072 * phases are precise.
15073 *
15074 * @param timeInterval time interval between epochs expressed in seconds (s).
15075 * @param oldPosition previous curvilinear position expressed in terms of latitude,
15076 * longitude and height.
15077 * @param oldC previous body-to-NED coordinate transformation.
15078 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
15079 * resolved along NED-frame axes.
15080 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
15081 * resolved along NED-frame axes.
15082 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
15083 * resolved along NED-frame axes.
15084 * @param kinematics body kinematics containing specific forces and angular rates applied to
15085 * the body.
15086 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
15087 * @param result instance where new estimated NED frame containing new body position,
15088 * velocity and coordinate transformation matrix will be stored.
15089 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
15090 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
15091 * body-to-NED-frame coordinate transformation matrix are
15092 * invalid.
15093 */
15094 public static void navigateNED(
15095 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
15096 final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD, final BodyKinematics kinematics,
15097 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
15098 InvalidSourceAndDestinationFrameTypeException {
15099 navigateNED(timeInterval, oldPosition.getLatitude(), oldPosition.getLongitude(), oldPosition.getHeight(), oldC,
15100 oldSpeedN, oldSpeedE, oldSpeedD, kinematics, accuracyThreshold, result);
15101 }
15102
15103 /**
15104 * Runs precision local-navigation-frame inertial navigation equations.
15105 * NOTE: only the attitude update and specific force frame transformation
15106 * phases are precise.
15107 *
15108 * @param timeInterval time interval between epochs expressed in seconds (s).
15109 * @param oldPosition previous curvilinear position expressed in terms of latitude,
15110 * longitude and height.
15111 * @param oldC previous body-to-NED coordinate transformation.
15112 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
15113 * resolved along NED-frame axes.
15114 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
15115 * resolved along NED-frame axes.
15116 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
15117 * resolved along NED-frame axes.
15118 * @param kinematics body kinematics containing specific forces and angular rates applied to
15119 * the body.
15120 * @param result instance where new estimated NED frame containing new body position,
15121 * velocity and coordinate transformation matrix will be stored.
15122 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
15123 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
15124 * body-to-NED-frame coordinate transformation matrix are
15125 * invalid.
15126 */
15127 public static void navigateNED(
15128 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
15129 final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD, final BodyKinematics kinematics,
15130 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
15131 navigateNED(timeInterval, oldPosition, oldC, oldSpeedN, oldSpeedE, oldSpeedD, kinematics,
15132 DEFAULT_ACCURACY_THRESHOLD, result);
15133 }
15134
15135 /**
15136 * Runs precision local-navigation-frame inertial navigation equations.
15137 * NOTE: only the attitude update and specific force frame transformation
15138 * phases are precise.
15139 *
15140 * @param timeInterval time interval between epochs.
15141 * @param oldPosition previous curvilinear position expressed in terms of latitude,
15142 * longitude and height.
15143 * @param oldC previous body-to-NED coordinate transformation.
15144 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
15145 * resolved along NED-frame axes.
15146 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
15147 * resolved along NED-frame axes.
15148 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
15149 * resolved along NED-frame axes.
15150 * @param kinematics body kinematics containing specific forces and angular rates applied to
15151 * the body.
15152 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
15153 * @param result instance where new estimated NED frame containing new body position,
15154 * velocity and coordinate transformation matrix will be stored.
15155 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
15156 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
15157 * body-to-NED-frame coordinate transformation matrix are
15158 * invalid.
15159 */
15160 public static void navigateNED(
15161 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
15162 final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD, final BodyKinematics kinematics,
15163 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
15164 InvalidSourceAndDestinationFrameTypeException {
15165 navigateNED(convertTimeToDouble(timeInterval), oldPosition, oldC, oldSpeedN, oldSpeedE, oldSpeedD, kinematics,
15166 accuracyThreshold, result);
15167 }
15168
15169 /**
15170 * Runs precision local-navigation-frame inertial navigation equations.
15171 * NOTE: only the attitude update and specific force frame transformation
15172 * phases are precise.
15173 *
15174 * @param timeInterval time interval between epochs.
15175 * @param oldPosition previous curvilinear position expressed in terms of latitude,
15176 * longitude and height.
15177 * @param oldC previous body-to-NED coordinate transformation.
15178 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
15179 * resolved along NED-frame axes.
15180 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
15181 * resolved along NED-frame axes.
15182 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
15183 * resolved along NED-frame axes.
15184 * @param kinematics body kinematics containing specific forces and angular rates applied to
15185 * the body.
15186 * @param result instance where new estimated NED frame containing new body position,
15187 * velocity and coordinate transformation matrix will be stored.
15188 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
15189 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
15190 * body-to-NED-frame coordinate transformation matrix are
15191 * invalid.
15192 */
15193 public static void navigateNED(
15194 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
15195 final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD, final BodyKinematics kinematics,
15196 final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
15197 navigateNED(timeInterval, oldPosition, oldC, oldSpeedN, oldSpeedE, oldSpeedD, kinematics,
15198 DEFAULT_ACCURACY_THRESHOLD, result);
15199 }
15200
15201 /**
15202 * Runs precision local-navigation-frame inertial navigation equations.
15203 * NOTE: only the attitude update and specific force frame transformation
15204 * phases are precise.
15205 *
15206 * @param timeInterval time interval between epochs expressed in seconds (s).
15207 * @param oldLatitude previous latitude expressed in radians (rad).
15208 * @param oldLongitude previous longitude expressed in radians (rad).
15209 * @param oldHeight previous height expressed in meters (m).
15210 * @param oldC previous body-to-NED coordinate transformation.
15211 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
15212 * resolved along NED-frame axes and expressed in meters per second (m/s).
15213 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
15214 * resolved along NED-frame axes and expressed in meters per second (m/s).
15215 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
15216 * resolved along NED-frame axes and expressed in meters per second (m/s).
15217 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
15218 * resolved along body-frame axes, averaged over time interval.
15219 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
15220 * resolved along body-frame axes, averaged over time interval.
15221 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
15222 * resolved along body-frame axes, averaged over time interval.
15223 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
15224 * resolved along body-frame axes, averaged over time interval and
15225 * expressed in radians per second (rad/s).
15226 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
15227 * resolved along body-frame axes, averaged over time interval and
15228 * expressed in radians per second (rad/s).
15229 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
15230 * resolved along body-frame axes, averaged over time interval and
15231 * expressed in radians per second (rad/s).
15232 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
15233 * @param result instance where new estimated NED frame containing new body position,
15234 * velocity and coordinate transformation matrix will be stored.
15235 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
15236 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
15237 * body-to-NED-frame coordinate transformation matrix are
15238 * invalid.
15239 */
15240 public static void navigateNED(
15241 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
15242 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
15243 final Acceleration fx, final Acceleration fy, final Acceleration fz,
15244 final double angularRateX, final double angularRateY, final double angularRateZ,
15245 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
15246 InvalidSourceAndDestinationFrameTypeException {
15247 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
15248 convertAccelerationToDouble(fx), convertAccelerationToDouble(fy), convertAccelerationToDouble(fz),
15249 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
15250 }
15251
15252 /**
15253 * Runs precision local-navigation-frame inertial navigation equations.
15254 * NOTE: only the attitude update and specific force frame transformation
15255 * phases are precise.
15256 *
15257 * @param timeInterval time interval between epochs.
15258 * @param oldLatitude previous latitude expressed in radians (rad).
15259 * @param oldLongitude previous longitude expressed in radians (rad).
15260 * @param oldHeight previous height expressed in meters (m).
15261 * @param oldC previous body-to-NED coordinate transformation.
15262 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
15263 * resolved along NED-frame axes and expressed in meters per second (m/s).
15264 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
15265 * resolved along NED-frame axes and expressed in meters per second (m/s).
15266 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
15267 * resolved along NED-frame axes and expressed in meters per second (m/s).
15268 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
15269 * resolved along body-frame axes, averaged over time interval.
15270 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
15271 * resolved along body-frame axes, averaged over time interval.
15272 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
15273 * resolved along body-frame axes, averaged over time interval.
15274 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
15275 * resolved along body-frame axes, averaged over time interval and
15276 * expressed in radians per second (rad/s).
15277 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
15278 * resolved along body-frame axes, averaged over time interval and
15279 * expressed in radians per second (rad/s).
15280 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
15281 * resolved along body-frame axes, averaged over time interval and
15282 * expressed in radians per second (rad/s).
15283 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
15284 * @param result instance where new estimated NED frame containing new body position,
15285 * velocity and coordinate transformation matrix will be stored.
15286 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
15287 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
15288 * body-to-NED-frame coordinate transformation matrix are
15289 * invalid.
15290 */
15291 public static void navigateNED(
15292 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
15293 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
15294 final Acceleration fx, final Acceleration fy, final Acceleration fz,
15295 final double angularRateX, final double angularRateY, final double angularRateZ,
15296 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
15297 InvalidSourceAndDestinationFrameTypeException {
15298 navigateNED(convertTimeToDouble(timeInterval), oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
15299 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
15300 }
15301
15302 /**
15303 * Runs precision local-navigation-frame inertial navigation equations.
15304 * NOTE: only the attitude update and specific force frame transformation
15305 * phases are precise.
15306 *
15307 * @param timeInterval time interval between epochs expressed in seconds (s).
15308 * @param oldPosition previous curvilinear position expressed in terms of latitude,
15309 * longitude and height.
15310 * @param oldC previous body-to-NED coordinate transformation.
15311 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
15312 * resolved along NED-frame axes and expressed in meters per second (m/s).
15313 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
15314 * resolved along NED-frame axes and expressed in meters per second (m/s).
15315 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
15316 * resolved along NED-frame axes and expressed in meters per second (m/s).
15317 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
15318 * resolved along body-frame axes, averaged over time interval.
15319 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
15320 * resolved along body-frame axes, averaged over time interval.
15321 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
15322 * resolved along body-frame axes, averaged over time interval.
15323 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
15324 * resolved along body-frame axes, averaged over time interval and
15325 * expressed in radians per second (rad/s).
15326 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
15327 * resolved along body-frame axes, averaged over time interval and
15328 * expressed in radians per second (rad/s).
15329 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
15330 * resolved along body-frame axes, averaged over time interval and
15331 * expressed in radians per second (rad/s).
15332 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
15333 * @param result instance where new estimated NED frame containing new body position,
15334 * velocity and coordinate transformation matrix will be stored.
15335 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
15336 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
15337 * body-to-NED-frame coordinate transformation matrix are
15338 * invalid.
15339 */
15340 public static void navigateNED(
15341 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
15342 final double oldVn, final double oldVe, final double oldVd,
15343 final Acceleration fx, final Acceleration fy, final Acceleration fz,
15344 final double angularRateX, final double angularRateY, final double angularRateZ,
15345 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
15346 InvalidSourceAndDestinationFrameTypeException {
15347 navigateNED(timeInterval, oldPosition.getLatitude(), oldPosition.getLongitude(), oldPosition.getHeight(),
15348 oldC, oldVn, oldVe, oldVd, fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold,
15349 result);
15350 }
15351
15352 /**
15353 * Runs precision local-navigation-frame inertial navigation equations.
15354 * NOTE: only the attitude update and specific force frame transformation
15355 * phases are precise.
15356 *
15357 * @param timeInterval time interval between epochs expressed in seconds (s).
15358 * @param oldPosition previous curvilinear position expressed in terms of latitude,
15359 * longitude and height.
15360 * @param oldC previous body-to-NED coordinate transformation.
15361 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
15362 * resolved along NED-frame axes and expressed in meters per second (m/s).
15363 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
15364 * resolved along NED-frame axes and expressed in meters per second (m/s).
15365 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
15366 * resolved along NED-frame axes and expressed in meters per second (m/s).
15367 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
15368 * resolved along body-frame axes, averaged over time interval.
15369 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
15370 * resolved along body-frame axes, averaged over time interval.
15371 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
15372 * resolved along body-frame axes, averaged over time interval.
15373 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
15374 * resolved along body-frame axes, averaged over time interval and
15375 * expressed in radians per second (rad/s).
15376 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
15377 * resolved along body-frame axes, averaged over time interval and
15378 * expressed in radians per second (rad/s).
15379 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
15380 * resolved along body-frame axes, averaged over time interval and
15381 * expressed in radians per second (rad/s).
15382 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
15383 * @param result instance where new estimated NED frame containing new body position,
15384 * velocity and coordinate transformation matrix will be stored.
15385 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
15386 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
15387 * body-to-NED-frame coordinate transformation matrix are
15388 * invalid.
15389 */
15390 public static void navigateNED(
15391 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
15392 final double oldVn, final double oldVe, final double oldVd,
15393 final Acceleration fx, final Acceleration fy, final Acceleration fz,
15394 final double angularRateX, final double angularRateY, final double angularRateZ,
15395 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
15396 InvalidSourceAndDestinationFrameTypeException {
15397 navigateNED(convertTimeToDouble(timeInterval), oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
15398 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
15399 }
15400
15401 /**
15402 * Runs precision local-navigation-frame inertial navigation equations.
15403 * NOTE: only the attitude update and specific force frame transformation
15404 * phases are precise.
15405 *
15406 * @param timeInterval time interval between epochs expressed in seconds (s).
15407 * @param oldLatitude previous latitude expressed in radians (rad).
15408 * @param oldLongitude previous longitude expressed in radians (rad).
15409 * @param oldHeight previous height expressed in meters (m).
15410 * @param oldC previous body-to-NED coordinate transformation.
15411 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
15412 * along north, east and down axes.
15413 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
15414 * resolved along body-frame axes, averaged over time interval.
15415 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
15416 * resolved along body-frame axes, averaged over time interval.
15417 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
15418 * resolved along body-frame axes, averaged over time interval.
15419 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
15420 * resolved along body-frame axes, averaged over time interval and
15421 * expressed in radians per second (rad/s).
15422 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
15423 * resolved along body-frame axes, averaged over time interval and
15424 * expressed in radians per second (rad/s).
15425 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
15426 * resolved along body-frame axes, averaged over time interval and
15427 * expressed in radians per second (rad/s).
15428 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
15429 * @param result instance where new estimated NED frame containing new body position,
15430 * velocity and coordinate transformation matrix will be stored.
15431 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
15432 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
15433 * body-to-NED-frame coordinate transformation matrix are
15434 * invalid.
15435 */
15436 public static void navigateNED(
15437 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
15438 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
15439 final Acceleration fx, final Acceleration fy, final Acceleration fz,
15440 final double angularRateX, final double angularRateY, final double angularRateZ,
15441 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
15442 InvalidSourceAndDestinationFrameTypeException {
15443 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
15444 oldVelocity.getVn(), oldVelocity.getVe(), oldVelocity.getVd(), fx, fy, fz,
15445 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
15446 }
15447
15448 /**
15449 * Runs precision local-navigation-frame inertial navigation equations.
15450 * NOTE: only the attitude update and specific force frame transformation
15451 * phases are precise.
15452 *
15453 * @param timeInterval time interval between epochs.
15454 * @param oldLatitude previous latitude expressed in radians (rad).
15455 * @param oldLongitude previous longitude expressed in radians (rad).
15456 * @param oldHeight previous height expressed in meters (m).
15457 * @param oldC previous body-to-NED coordinate transformation.
15458 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
15459 * along north, east and down axes.
15460 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
15461 * resolved along body-frame axes, averaged over time interval.
15462 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
15463 * resolved along body-frame axes, averaged over time interval.
15464 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
15465 * resolved along body-frame axes, averaged over time interval.
15466 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
15467 * resolved along body-frame axes, averaged over time interval and
15468 * expressed in radians per second (rad/s).
15469 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
15470 * resolved along body-frame axes, averaged over time interval and
15471 * expressed in radians per second (rad/s).
15472 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
15473 * resolved along body-frame axes, averaged over time interval and
15474 * expressed in radians per second (rad/s).
15475 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
15476 * @param result instance where new estimated NED frame containing new body position,
15477 * velocity and coordinate transformation matrix will be stored.
15478 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
15479 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
15480 * body-to-NED-frame coordinate transformation matrix are
15481 * invalid.
15482 */
15483 public static void navigateNED(
15484 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
15485 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
15486 final Acceleration fx, final Acceleration fy, final Acceleration fz,
15487 final double angularRateX, final double angularRateY, final double angularRateZ,
15488 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
15489 InvalidSourceAndDestinationFrameTypeException {
15490 navigateNED(convertTimeToDouble(timeInterval), oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity,
15491 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
15492 }
15493
15494 /**
15495 * Runs precision local-navigation-frame inertial navigation equations.
15496 * NOTE: only the attitude update and specific force frame transformation
15497 * phases are precise.
15498 *
15499 * @param timeInterval time interval between epochs expressed in seconds (s).
15500 * @param oldPosition previous curvilinear position expressed in terms of latitude,
15501 * longitude and height.
15502 * @param oldC previous body-to-NED coordinate transformation.
15503 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
15504 * along north, east and down axes.
15505 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
15506 * resolved along body-frame axes, averaged over time interval.
15507 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
15508 * resolved along body-frame axes, averaged over time interval.
15509 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
15510 * resolved along body-frame axes, averaged over time interval.
15511 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
15512 * resolved along body-frame axes, averaged over time interval and
15513 * expressed in radians per second (rad/s).
15514 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
15515 * resolved along body-frame axes, averaged over time interval and
15516 * expressed in radians per second (rad/s).
15517 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
15518 * resolved along body-frame axes, averaged over time interval and
15519 * expressed in radians per second (rad/s).
15520 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
15521 * @param result instance where new estimated NED frame containing new body position,
15522 * velocity and coordinate transformation matrix will be stored.
15523 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
15524 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
15525 * body-to-NED-frame coordinate transformation matrix are
15526 * invalid.
15527 */
15528 public static void navigateNED(
15529 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
15530 final NEDVelocity oldVelocity, final Acceleration fx, final Acceleration fy, final Acceleration fz,
15531 final double angularRateX, final double angularRateY, final double angularRateZ,
15532 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
15533 InvalidSourceAndDestinationFrameTypeException {
15534 navigateNED(timeInterval, oldPosition, oldC, oldVelocity.getVn(), oldVelocity.getVe(), oldVelocity.getVd(),
15535 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
15536 }
15537
15538 /**
15539 * Runs precision local-navigation-frame inertial navigation equations.
15540 * NOTE: only the attitude update and specific force frame transformation
15541 * phases are precise.
15542 *
15543 * @param timeInterval time interval between epochs.
15544 * @param oldPosition previous curvilinear position expressed in terms of latitude,
15545 * longitude and height.
15546 * @param oldC previous body-to-NED coordinate transformation.
15547 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
15548 * along north, east and down axes.
15549 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
15550 * resolved along body-frame axes, averaged over time interval.
15551 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
15552 * resolved along body-frame axes, averaged over time interval.
15553 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
15554 * resolved along body-frame axes, averaged over time interval.
15555 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
15556 * resolved along body-frame axes, averaged over time interval and
15557 * expressed in radians per second (rad/s).
15558 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
15559 * resolved along body-frame axes, averaged over time interval and
15560 * expressed in radians per second (rad/s).
15561 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
15562 * resolved along body-frame axes, averaged over time interval and
15563 * expressed in radians per second (rad/s).
15564 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
15565 * @param result instance where new estimated NED frame containing new body position,
15566 * velocity and coordinate transformation matrix will be stored.
15567 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
15568 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
15569 * body-to-NED-frame coordinate transformation matrix are
15570 * invalid.
15571 */
15572 public static void navigateNED(
15573 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
15574 final NEDVelocity oldVelocity, final Acceleration fx, final Acceleration fy, final Acceleration fz,
15575 final double angularRateX, final double angularRateY, final double angularRateZ,
15576 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
15577 InvalidSourceAndDestinationFrameTypeException {
15578 navigateNED(convertTimeToDouble(timeInterval), oldPosition, oldC, oldVelocity, fx, fy, fz,
15579 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
15580 }
15581
15582 /**
15583 * Runs precision local-navigation-frame inertial navigation equations.
15584 * NOTE: only the attitude update and specific force frame transformation
15585 * phases are precise.
15586 *
15587 * @param timeInterval time interval between epochs expressed in seconds (s).
15588 * @param oldLatitude previous latitude expressed in radians (rad).
15589 * @param oldLongitude previous longitude expressed in radians (rad).
15590 * @param oldHeight previous height expressed in meters (m).
15591 * @param oldC previous body-to-NED coordinate transformation.
15592 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
15593 * resolved along NED-frame axes and expressed in meters per second (m/s).
15594 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
15595 * resolved along NED-frame axes and expressed in meters per second (m/s).
15596 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
15597 * resolved along NED-frame axes and expressed in meters per second (m/s).
15598 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
15599 * resolved along body-frame axes, averaged over time interval and
15600 * expressed in meters per squared second (m/s^2).
15601 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
15602 * resolved along body-frame axes, averaged over time interval and
15603 * expressed in meters per squared second (m/s^2).
15604 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
15605 * resolved along body-frame axes, averaged over time interval and
15606 * expressed in meters per squared second (m/s^2).
15607 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
15608 * resolved along body-frame axes, averaged over time interval.
15609 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
15610 * resolved along body-frame axes, averaged over time interval.
15611 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
15612 * resolved along body-frame axes, averaged over time interval.
15613 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
15614 * @param result instance where new estimated NED frame containing new body position,
15615 * velocity and coordinate transformation matrix will be stored.
15616 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
15617 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
15618 * body-to-NED-frame coordinate transformation matrix are
15619 * invalid.
15620 */
15621 public static void navigateNED(
15622 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
15623 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
15624 final double fx, final double fy, final double fz,
15625 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
15626 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
15627 InvalidSourceAndDestinationFrameTypeException {
15628 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
15629 convertAngularSpeedToDouble(angularRateX), convertAngularSpeedToDouble(angularRateY),
15630 convertAngularSpeedToDouble(angularRateZ), accuracyThreshold, result);
15631 }
15632
15633 /**
15634 * Runs precision local-navigation-frame inertial navigation equations.
15635 * NOTE: only the attitude update and specific force frame transformation
15636 * phases are precise.
15637 *
15638 * @param timeInterval time interval between epochs.
15639 * @param oldLatitude previous latitude expressed in radians (rad).
15640 * @param oldLongitude previous longitude expressed in radians (rad).
15641 * @param oldHeight previous height expressed in meters (m).
15642 * @param oldC previous body-to-NED coordinate transformation.
15643 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
15644 * resolved along NED-frame axes and expressed in meters per second (m/s).
15645 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
15646 * resolved along NED-frame axes and expressed in meters per second (m/s).
15647 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
15648 * resolved along NED-frame axes and expressed in meters per second (m/s).
15649 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
15650 * resolved along body-frame axes, averaged over time interval and
15651 * expressed in meters per squared second (m/s^2).
15652 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
15653 * resolved along body-frame axes, averaged over time interval and
15654 * expressed in meters per squared second (m/s^2).
15655 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
15656 * resolved along body-frame axes, averaged over time interval and
15657 * expressed in meters per squared second (m/s^2).
15658 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
15659 * resolved along body-frame axes, averaged over time interval.
15660 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
15661 * resolved along body-frame axes, averaged over time interval.
15662 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
15663 * resolved along body-frame axes, averaged over time interval.
15664 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
15665 * @param result instance where new estimated NED frame containing new body position,
15666 * velocity and coordinate transformation matrix will be stored.
15667 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
15668 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
15669 * body-to-NED-frame coordinate transformation matrix are
15670 * invalid.
15671 */
15672 public static void navigateNED(
15673 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
15674 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
15675 final double fx, final double fy, final double fz,
15676 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
15677 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
15678 InvalidSourceAndDestinationFrameTypeException {
15679 navigateNED(convertTimeToDouble(timeInterval), oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
15680 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
15681 }
15682
15683 /**
15684 * Runs precision local-navigation-frame inertial navigation equations.
15685 * NOTE: only the attitude update and specific force frame transformation
15686 * phases are precise.
15687 *
15688 * @param timeInterval time interval between epochs expressed in seconds (s).
15689 * @param oldPosition previous curvilinear position expressed in terms of latitude,
15690 * longitude and height.
15691 * @param oldC previous body-to-NED coordinate transformation.
15692 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
15693 * resolved along NED-frame axes and expressed in meters per second (m/s).
15694 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
15695 * resolved along NED-frame axes and expressed in meters per second (m/s).
15696 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
15697 * resolved along NED-frame axes and expressed in meters per second (m/s).
15698 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
15699 * resolved along body-frame axes, averaged over time interval and
15700 * expressed in meters per squared second (m/s^2).
15701 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
15702 * resolved along body-frame axes, averaged over time interval and
15703 * expressed in meters per squared second (m/s^2).
15704 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
15705 * resolved along body-frame axes, averaged over time interval and
15706 * expressed in meters per squared second (m/s^2).
15707 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
15708 * resolved along body-frame axes, averaged over time interval.
15709 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
15710 * resolved along body-frame axes, averaged over time interval.
15711 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
15712 * resolved along body-frame axes, averaged over time interval.
15713 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
15714 * @param result instance where new estimated NED frame containing new body position,
15715 * velocity and coordinate transformation matrix will be stored.
15716 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
15717 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
15718 * body-to-NED-frame coordinate transformation matrix are
15719 * invalid.
15720 */
15721 public static void navigateNED(
15722 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
15723 final double oldVn, final double oldVe, final double oldVd,
15724 final double fx, final double fy, final double fz,
15725 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
15726 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
15727 InvalidSourceAndDestinationFrameTypeException {
15728 navigateNED(timeInterval, oldPosition.getLatitude(), oldPosition.getLongitude(), oldPosition.getHeight(), oldC,
15729 oldVn, oldVe, oldVd, fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
15730 }
15731
15732 /**
15733 * Runs precision local-navigation-frame inertial navigation equations.
15734 * NOTE: only the attitude update and specific force frame transformation
15735 * phases are precise.
15736 *
15737 * @param timeInterval time interval between epochs.
15738 * @param oldPosition previous curvilinear position expressed in terms of latitude,
15739 * longitude and height.
15740 * @param oldC previous body-to-NED coordinate transformation.
15741 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
15742 * resolved along NED-frame axes and expressed in meters per second (m/s).
15743 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
15744 * resolved along NED-frame axes and expressed in meters per second (m/s).
15745 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
15746 * resolved along NED-frame axes and expressed in meters per second (m/s).
15747 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
15748 * resolved along body-frame axes, averaged over time interval and
15749 * expressed in meters per squared second (m/s^2).
15750 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
15751 * resolved along body-frame axes, averaged over time interval and
15752 * expressed in meters per squared second (m/s^2).
15753 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
15754 * resolved along body-frame axes, averaged over time interval and
15755 * expressed in meters per squared second (m/s^2).
15756 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
15757 * resolved along body-frame axes, averaged over time interval.
15758 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
15759 * resolved along body-frame axes, averaged over time interval.
15760 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
15761 * resolved along body-frame axes, averaged over time interval.
15762 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
15763 * @param result instance where new estimated NED frame containing new body position,
15764 * velocity and coordinate transformation matrix will be stored.
15765 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
15766 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
15767 * body-to-NED-frame coordinate transformation matrix are
15768 * invalid.
15769 */
15770 public static void navigateNED(
15771 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
15772 final double oldVn, final double oldVe, final double oldVd,
15773 final double fx, final double fy, final double fz,
15774 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
15775 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
15776 InvalidSourceAndDestinationFrameTypeException {
15777 navigateNED(convertTimeToDouble(timeInterval), oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
15778 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
15779 }
15780
15781 /**
15782 * Runs precision local-navigation-frame inertial navigation equations.
15783 * NOTE: only the attitude update and specific force frame transformation
15784 * phases are precise.
15785 *
15786 * @param timeInterval time interval between epochs expressed in seconds (s).
15787 * @param oldLatitude previous latitude expressed in radians (rad).
15788 * @param oldLongitude previous longitude expressed in radians (rad).
15789 * @param oldHeight previous height expressed in meters (m).
15790 * @param oldC previous body-to-NED coordinate transformation.
15791 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
15792 * along north, east and down axes.
15793 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
15794 * resolved along body-frame axes, averaged over time interval and
15795 * expressed in meters per squared second (m/s^2).
15796 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
15797 * resolved along body-frame axes, averaged over time interval and
15798 * expressed in meters per squared second (m/s^2).
15799 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
15800 * resolved along body-frame axes, averaged over time interval and
15801 * expressed in meters per squared second (m/s^2).
15802 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
15803 * resolved along body-frame axes, averaged over time interval.
15804 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
15805 * resolved along body-frame axes, averaged over time interval.
15806 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
15807 * resolved along body-frame axes, averaged over time interval.
15808 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
15809 * @param result instance where new estimated NED frame containing new body position,
15810 * velocity and coordinate transformation matrix will be stored.
15811 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
15812 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
15813 * body-to-NED-frame coordinate transformation matrix are
15814 * invalid.
15815 */
15816 public static void navigateNED(
15817 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
15818 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
15819 final double fx, final double fy, final double fz,
15820 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
15821 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
15822 InvalidSourceAndDestinationFrameTypeException {
15823 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
15824 oldVelocity.getVn(), oldVelocity.getVe(), oldVelocity.getVd(), fx, fy, fz,
15825 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
15826 }
15827
15828 /**
15829 * Runs precision local-navigation-frame inertial navigation equations.
15830 * NOTE: only the attitude update and specific force frame transformation
15831 * phases are precise.
15832 *
15833 * @param timeInterval time interval between epochs.
15834 * @param oldLatitude previous latitude expressed in radians (rad).
15835 * @param oldLongitude previous longitude expressed in radians (rad).
15836 * @param oldHeight previous height expressed in meters (m).
15837 * @param oldC previous body-to-NED coordinate transformation.
15838 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
15839 * along north, east and down axes.
15840 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
15841 * resolved along body-frame axes, averaged over time interval and
15842 * expressed in meters per squared second (m/s^2).
15843 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
15844 * resolved along body-frame axes, averaged over time interval and
15845 * expressed in meters per squared second (m/s^2).
15846 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
15847 * resolved along body-frame axes, averaged over time interval and
15848 * expressed in meters per squared second (m/s^2).
15849 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
15850 * resolved along body-frame axes, averaged over time interval.
15851 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
15852 * resolved along body-frame axes, averaged over time interval.
15853 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
15854 * resolved along body-frame axes, averaged over time interval.
15855 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
15856 * @param result instance where new estimated NED frame containing new body position,
15857 * velocity and coordinate transformation matrix will be stored.
15858 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
15859 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
15860 * body-to-NED-frame coordinate transformation matrix are
15861 * invalid.
15862 */
15863 public static void navigateNED(
15864 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
15865 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
15866 final double fx, final double fy, final double fz,
15867 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
15868 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
15869 InvalidSourceAndDestinationFrameTypeException {
15870 navigateNED(convertTimeToDouble(timeInterval), oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity,
15871 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
15872 }
15873
15874 /**
15875 * Runs precision local-navigation-frame inertial navigation equations.
15876 * NOTE: only the attitude update and specific force frame transformation
15877 * phases are precise.
15878 *
15879 * @param timeInterval time interval between epochs expressed in seconds (s).
15880 * @param oldPosition previous curvilinear position expressed in terms of latitude,
15881 * longitude and height.
15882 * @param oldC previous body-to-NED coordinate transformation.
15883 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
15884 * along north, east and down axes.
15885 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
15886 * resolved along body-frame axes, averaged over time interval and
15887 * expressed in meters per squared second (m/s^2).
15888 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
15889 * resolved along body-frame axes, averaged over time interval and
15890 * expressed in meters per squared second (m/s^2).
15891 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
15892 * resolved along body-frame axes, averaged over time interval and
15893 * expressed in meters per squared second (m/s^2).
15894 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
15895 * resolved along body-frame axes, averaged over time interval.
15896 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
15897 * resolved along body-frame axes, averaged over time interval.
15898 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
15899 * resolved along body-frame axes, averaged over time interval.
15900 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
15901 * @param result instance where new estimated NED frame containing new body position,
15902 * velocity and coordinate transformation matrix will be stored.
15903 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
15904 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
15905 * body-to-NED-frame coordinate transformation matrix are
15906 * invalid.
15907 */
15908 public static void navigateNED(
15909 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
15910 final NEDVelocity oldVelocity, final double fx, final double fy, final double fz,
15911 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
15912 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
15913 InvalidSourceAndDestinationFrameTypeException {
15914 navigateNED(timeInterval, oldPosition, oldC, oldVelocity.getVn(), oldVelocity.getVe(), oldVelocity.getVd(),
15915 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
15916 }
15917
15918 /**
15919 * Runs precision local-navigation-frame inertial navigation equations.
15920 * NOTE: only the attitude update and specific force frame transformation
15921 * phases are precise.
15922 *
15923 * @param timeInterval time interval between epochs.
15924 * @param oldPosition previous curvilinear position expressed in terms of latitude,
15925 * longitude and height.
15926 * @param oldC previous body-to-NED coordinate transformation.
15927 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
15928 * along north, east and down axes.
15929 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
15930 * resolved along body-frame axes, averaged over time interval and
15931 * expressed in meters per squared second (m/s^2).
15932 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
15933 * resolved along body-frame axes, averaged over time interval and
15934 * expressed in meters per squared second (m/s^2).
15935 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
15936 * resolved along body-frame axes, averaged over time interval and
15937 * expressed in meters per squared second (m/s^2).
15938 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
15939 * resolved along body-frame axes, averaged over time interval.
15940 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
15941 * resolved along body-frame axes, averaged over time interval.
15942 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
15943 * resolved along body-frame axes, averaged over time interval.
15944 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
15945 * @param result instance where new estimated NED frame containing new body position,
15946 * velocity and coordinate transformation matrix will be stored.
15947 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
15948 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
15949 * body-to-NED-frame coordinate transformation matrix are
15950 * invalid.
15951 */
15952 public static void navigateNED(
15953 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
15954 final NEDVelocity oldVelocity, final double fx, final double fy, final double fz,
15955 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
15956 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
15957 InvalidSourceAndDestinationFrameTypeException {
15958 navigateNED(convertTimeToDouble(timeInterval), oldPosition, oldC, oldVelocity, fx, fy, fz,
15959 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
15960 }
15961
15962 /**
15963 * Runs precision local-navigation-frame inertial navigation equations.
15964 * NOTE: only the attitude update and specific force frame transformation
15965 * phases are precise.
15966 *
15967 * @param timeInterval time interval between epochs expressed in seconds (s).
15968 * @param oldLatitude previous latitude angle.
15969 * @param oldLongitude previous longitude angle.
15970 * @param oldHeight previous height.
15971 * @param oldC previous body-to-NED coordinate transformation.
15972 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
15973 * resolved along NED-frame axes.
15974 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
15975 * resolved along NED-frame axes.
15976 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
15977 * resolved along NED-frame axes.
15978 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
15979 * resolved along body-frame axes, averaged over time interval and
15980 * expressed in meters per squared second (m/s^2).
15981 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
15982 * resolved along body-frame axes, averaged over time interval and
15983 * expressed in meters per squared second (m/s^2).
15984 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
15985 * resolved along body-frame axes, averaged over time interval and
15986 * expressed in meters per squared second (m/s^2).
15987 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
15988 * resolved along body-frame axes, averaged over time interval and
15989 * expressed in radians per second (rad/s).
15990 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
15991 * resolved along body-frame axes, averaged over time interval and
15992 * expressed in radians per second (rad/s).
15993 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
15994 * resolved along body-frame axes, averaged over time interval and
15995 * expressed in radians per second (rad/s).
15996 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
15997 * @param result instance where new estimated NED frame containing new body position,
15998 * velocity and coordinate transformation matrix will be stored.
15999 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
16000 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
16001 * body-to-NED-frame coordinate transformation matrix are
16002 * invalid.
16003 */
16004 public static void navigateNED(
16005 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
16006 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
16007 final double fx, final double fy, final double fz,
16008 final double angularRateX, final double angularRateY, final double angularRateZ,
16009 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
16010 InvalidSourceAndDestinationFrameTypeException {
16011 navigateNED(timeInterval, convertAngleToDouble(oldLatitude), convertAngleToDouble(oldLongitude),
16012 convertDistanceToDouble(oldHeight), oldC, convertSpeedToDouble(oldSpeedN),
16013 convertSpeedToDouble(oldSpeedE), convertSpeedToDouble(oldSpeedD), fx, fy, fz,
16014 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
16015 }
16016
16017 /**
16018 * Runs precision local-navigation-frame inertial navigation equations.
16019 * NOTE: only the attitude update and specific force frame transformation
16020 * phases are precise.
16021 *
16022 * @param timeInterval time interval between epochs.
16023 * @param oldLatitude previous latitude angle.
16024 * @param oldLongitude previous longitude angle.
16025 * @param oldHeight previous height.
16026 * @param oldC previous body-to-NED coordinate transformation.
16027 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
16028 * resolved along NED-frame axes.
16029 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
16030 * resolved along NED-frame axes.
16031 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
16032 * resolved along NED-frame axes.
16033 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
16034 * resolved along body-frame axes, averaged over time interval and
16035 * expressed in meters per squared second (m/s^2).
16036 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
16037 * resolved along body-frame axes, averaged over time interval and
16038 * expressed in meters per squared second (m/s^2).
16039 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
16040 * resolved along body-frame axes, averaged over time interval and
16041 * expressed in meters per squared second (m/s^2).
16042 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
16043 * resolved along body-frame axes, averaged over time interval and
16044 * expressed in radians per second (rad/s).
16045 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
16046 * resolved along body-frame axes, averaged over time interval and
16047 * expressed in radians per second (rad/s).
16048 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
16049 * resolved along body-frame axes, averaged over time interval and
16050 * expressed in radians per second (rad/s).
16051 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
16052 * @param result instance where new estimated NED frame containing new body position,
16053 * velocity and coordinate transformation matrix will be stored.
16054 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
16055 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
16056 * body-to-NED-frame coordinate transformation matrix are
16057 * invalid.
16058 */
16059 public static void navigateNED(
16060 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
16061 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
16062 final double fx, final double fy, final double fz,
16063 final double angularRateX, final double angularRateY, final double angularRateZ,
16064 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
16065 InvalidSourceAndDestinationFrameTypeException {
16066 navigateNED(convertTimeToDouble(timeInterval), oldLatitude, oldLongitude, oldHeight, oldC,
16067 oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
16068 accuracyThreshold, result);
16069 }
16070
16071 /**
16072 * Runs precision local-navigation-frame inertial navigation equations.
16073 * NOTE: only the attitude update and specific force frame transformation
16074 * phases are precise.
16075 *
16076 * @param timeInterval time interval between epochs expressed in seconds (s).
16077 * @param oldLatitude previous latitude angle.
16078 * @param oldLongitude previous longitude angle.
16079 * @param oldHeight previous height.
16080 * @param oldC previous body-to-NED coordinate transformation.
16081 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
16082 * resolved along NED-frame axes.
16083 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
16084 * resolved along NED-frame axes.
16085 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
16086 * resolved along NED-frame axes.
16087 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
16088 * resolved along body-frame axes, averaged over time interval.
16089 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
16090 * resolved along body-frame axes, averaged over time interval.
16091 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
16092 * resolved along body-frame axes, averaged over time interval.
16093 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
16094 * resolved along body-frame axes, averaged over time interval.
16095 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
16096 * resolved along body-frame axes, averaged over time interval.
16097 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
16098 * resolved along body-frame axes, averaged over time interval.
16099 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
16100 * @param result instance where new estimated NED frame containing new body position,
16101 * velocity and coordinate transformation matrix will be stored.
16102 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
16103 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
16104 * body-to-NED-frame coordinate transformation matrix are
16105 * invalid.
16106 */
16107 public static void navigateNED(
16108 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
16109 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
16110 final Acceleration fx, final Acceleration fy, final Acceleration fz,
16111 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
16112 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
16113 InvalidSourceAndDestinationFrameTypeException {
16114 navigateNED(timeInterval, convertAngleToDouble(oldLatitude), convertAngleToDouble(oldLongitude),
16115 convertDistanceToDouble(oldHeight), oldC, convertSpeedToDouble(oldSpeedN),
16116 convertSpeedToDouble(oldSpeedE), convertSpeedToDouble(oldSpeedD),
16117 convertAccelerationToDouble(fx), convertAccelerationToDouble(fy), convertAccelerationToDouble(fz),
16118 convertAngularSpeedToDouble(angularRateX), convertAngularSpeedToDouble(angularRateY),
16119 convertAngularSpeedToDouble(angularRateZ), accuracyThreshold, result);
16120 }
16121
16122 /**
16123 * Runs precision local-navigation-frame inertial navigation equations.
16124 * NOTE: only the attitude update and specific force frame transformation
16125 * phases are precise.
16126 *
16127 * @param timeInterval time interval between epochs.
16128 * @param oldLatitude previous latitude angle.
16129 * @param oldLongitude previous longitude angle.
16130 * @param oldHeight previous height.
16131 * @param oldC previous body-to-NED coordinate transformation.
16132 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
16133 * resolved along NED-frame axes.
16134 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
16135 * resolved along NED-frame axes.
16136 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
16137 * resolved along NED-frame axes.
16138 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
16139 * resolved along body-frame axes, averaged over time interval.
16140 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
16141 * resolved along body-frame axes, averaged over time interval.
16142 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
16143 * resolved along body-frame axes, averaged over time interval.
16144 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
16145 * resolved along body-frame axes, averaged over time interval.
16146 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
16147 * resolved along body-frame axes, averaged over time interval.
16148 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
16149 * resolved along body-frame axes, averaged over time interval.
16150 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
16151 * @param result instance where new estimated NED frame containing new body position,
16152 * velocity and coordinate transformation matrix will be stored.
16153 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
16154 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
16155 * body-to-NED-frame coordinate transformation matrix are
16156 * invalid.
16157 */
16158 public static void navigateNED(
16159 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
16160 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
16161 final Acceleration fx, final Acceleration fy, final Acceleration fz,
16162 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
16163 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
16164 InvalidSourceAndDestinationFrameTypeException {
16165 navigateNED(convertTimeToDouble(timeInterval), oldLatitude, oldLongitude, oldHeight, oldC,
16166 oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
16167 accuracyThreshold, result);
16168 }
16169
16170 /**
16171 * Runs precision local-navigation-frame inertial navigation equations.
16172 * NOTE: only the attitude update and specific force frame transformation
16173 * phases are precise.
16174 *
16175 * @param timeInterval time interval between epochs expressed in seconds (s).
16176 * @param oldLatitude previous latitude expressed in radians (rad).
16177 * @param oldLongitude previous longitude expressed in radians (rad).
16178 * @param oldHeight previous height expressed in meters (m).
16179 * @param oldC previous body-to-NED coordinate transformation.
16180 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
16181 * resolved along NED-frame axes.
16182 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
16183 * resolved along NED-frame axes.
16184 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
16185 * resolved along NED-frame axes.
16186 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
16187 * resolved along body-frame axes, averaged over time interval.
16188 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
16189 * resolved along body-frame axes, averaged over time interval.
16190 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
16191 * resolved along body-frame axes, averaged over time interval.
16192 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
16193 * resolved along body-frame axes, averaged over time interval.
16194 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
16195 * resolved along body-frame axes, averaged over time interval.
16196 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
16197 * resolved along body-frame axes, averaged over time interval.
16198 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
16199 * @param result instance where new estimated NED frame containing new body position,
16200 * velocity and coordinate transformation matrix will be stored.
16201 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
16202 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
16203 * body-to-NED-frame coordinate transformation matrix are
16204 * invalid.
16205 */
16206 public static void navigateNED(
16207 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
16208 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
16209 final Acceleration fx, final Acceleration fy, final Acceleration fz,
16210 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
16211 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
16212 InvalidSourceAndDestinationFrameTypeException {
16213 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, convertSpeedToDouble(oldSpeedN),
16214 convertSpeedToDouble(oldSpeedE), convertSpeedToDouble(oldSpeedD),
16215 convertAccelerationToDouble(fx), convertAccelerationToDouble(fy), convertAccelerationToDouble(fz),
16216 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
16217 }
16218
16219 /**
16220 * Runs precision local-navigation-frame inertial navigation equations.
16221 * NOTE: only the attitude update and specific force frame transformation
16222 * phases are precise.
16223 *
16224 * @param timeInterval time interval between epochs.
16225 * @param oldLatitude previous latitude expressed in radians (rad).
16226 * @param oldLongitude previous longitude expressed in radians (rad).
16227 * @param oldHeight previous height expressed in meters (m).
16228 * @param oldC previous body-to-NED coordinate transformation.
16229 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
16230 * resolved along NED-frame axes.
16231 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
16232 * resolved along NED-frame axes.
16233 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
16234 * resolved along NED-frame axes.
16235 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
16236 * resolved along body-frame axes, averaged over time interval.
16237 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
16238 * resolved along body-frame axes, averaged over time interval.
16239 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
16240 * resolved along body-frame axes, averaged over time interval.
16241 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
16242 * resolved along body-frame axes, averaged over time interval.
16243 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
16244 * resolved along body-frame axes, averaged over time interval.
16245 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
16246 * resolved along body-frame axes, averaged over time interval.
16247 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
16248 * @param result instance where new estimated NED frame containing new body position,
16249 * velocity and coordinate transformation matrix will be stored.
16250 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
16251 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
16252 * body-to-NED-frame coordinate transformation matrix are
16253 * invalid.
16254 */
16255 public static void navigateNED(
16256 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
16257 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
16258 final Acceleration fx, final Acceleration fy, final Acceleration fz,
16259 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
16260 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
16261 InvalidSourceAndDestinationFrameTypeException {
16262 navigateNED(convertTimeToDouble(timeInterval), oldLatitude, oldLongitude, oldHeight, oldC,
16263 oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
16264 accuracyThreshold, result);
16265 }
16266
16267 /**
16268 * Runs precision local-navigation-frame inertial navigation equations.
16269 * NOTE: only the attitude update and specific force frame transformation
16270 * phases are precise.
16271 *
16272 * @param timeInterval time interval between epochs expressed in seconds (s).
16273 * @param oldPosition previous curvilinear position expressed in terms of latitude,
16274 * longitude and height.
16275 * @param oldC previous body-to-NED coordinate transformation.
16276 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
16277 * resolved along NED-frame axes.
16278 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
16279 * resolved along NED-frame axes.
16280 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
16281 * resolved along NED-frame axes.
16282 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
16283 * resolved along body-frame axes, averaged over time interval.
16284 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
16285 * resolved along body-frame axes, averaged over time interval.
16286 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
16287 * resolved along body-frame axes, averaged over time interval.
16288 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
16289 * resolved along body-frame axes, averaged over time interval.
16290 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
16291 * resolved along body-frame axes, averaged over time interval.
16292 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
16293 * resolved along body-frame axes, averaged over time interval.
16294 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
16295 * @param result instance where new estimated NED frame containing new body position,
16296 * velocity and coordinate transformation matrix will be stored.
16297 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
16298 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
16299 * body-to-NED-frame coordinate transformation matrix are
16300 * invalid.
16301 */
16302 public static void navigateNED(
16303 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
16304 final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
16305 final Acceleration fx, final Acceleration fy, final Acceleration fz,
16306 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
16307 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
16308 InvalidSourceAndDestinationFrameTypeException {
16309 navigateNED(timeInterval, oldPosition.getLatitude(), oldPosition.getLongitude(), oldPosition.getHeight(), oldC,
16310 oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
16311 accuracyThreshold, result);
16312 }
16313
16314 /**
16315 * Runs precision local-navigation-frame inertial navigation equations.
16316 * NOTE: only the attitude update and specific force frame transformation
16317 * phases are precise.
16318 *
16319 * @param timeInterval time interval between epochs.
16320 * @param oldPosition previous curvilinear position expressed in terms of latitude,
16321 * longitude and height.
16322 * @param oldC previous body-to-NED coordinate transformation.
16323 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
16324 * resolved along NED-frame axes.
16325 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
16326 * resolved along NED-frame axes.
16327 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
16328 * resolved along NED-frame axes.
16329 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
16330 * resolved along body-frame axes, averaged over time interval.
16331 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
16332 * resolved along body-frame axes, averaged over time interval.
16333 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
16334 * resolved along body-frame axes, averaged over time interval.
16335 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
16336 * resolved along body-frame axes, averaged over time interval.
16337 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
16338 * resolved along body-frame axes, averaged over time interval.
16339 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
16340 * resolved along body-frame axes, averaged over time interval.
16341 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
16342 * @param result instance where new estimated NED frame containing new body position,
16343 * velocity and coordinate transformation matrix will be stored.
16344 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
16345 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
16346 * body-to-NED-frame coordinate transformation matrix are
16347 * invalid.
16348 */
16349 public static void navigateNED(
16350 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
16351 final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
16352 final Acceleration fx, final Acceleration fy, final Acceleration fz,
16353 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
16354 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
16355 InvalidSourceAndDestinationFrameTypeException {
16356 navigateNED(convertTimeToDouble(timeInterval), oldPosition, oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
16357 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
16358 }
16359
16360 /**
16361 * Runs precision local-navigation-frame inertial navigation equations.
16362 * NOTE: only the attitude update and specific force frame transformation
16363 * phases are precise.
16364 *
16365 * @param timeInterval time interval between epochs expressed in seconds (s).
16366 * @param oldLatitude previous latitude angle.
16367 * @param oldLongitude previous longitude angle.
16368 * @param oldHeight previous height.
16369 * @param oldC previous body-to-NED coordinate transformation.
16370 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
16371 * along north, east and down axes.
16372 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
16373 * resolved along body-frame axes, averaged over time interval.
16374 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
16375 * resolved along body-frame axes, averaged over time interval.
16376 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
16377 * resolved along body-frame axes, averaged over time interval.
16378 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
16379 * resolved along body-frame axes, averaged over time interval.
16380 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
16381 * resolved along body-frame axes, averaged over time interval.
16382 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
16383 * resolved along body-frame axes, averaged over time interval.
16384 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
16385 * @param result instance where new estimated NED frame containing new body position,
16386 * velocity and coordinate transformation matrix will be stored.
16387 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
16388 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
16389 * body-to-NED-frame coordinate transformation matrix are
16390 * invalid.
16391 */
16392 public static void navigateNED(
16393 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
16394 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
16395 final Acceleration fx, final Acceleration fy, final Acceleration fz,
16396 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
16397 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
16398 InvalidSourceAndDestinationFrameTypeException {
16399 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
16400 oldVelocity.getVn(), oldVelocity.getVe(), oldVelocity.getVd(), fx, fy, fz,
16401 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
16402 }
16403
16404 /**
16405 * Runs precision local-navigation-frame inertial navigation equations.
16406 * NOTE: only the attitude update and specific force frame transformation
16407 * phases are precise.
16408 *
16409 * @param timeInterval time interval between epochs.
16410 * @param oldLatitude previous latitude angle.
16411 * @param oldLongitude previous longitude angle.
16412 * @param oldHeight previous height.
16413 * @param oldC previous body-to-NED coordinate transformation.
16414 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
16415 * along north, east and down axes.
16416 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
16417 * resolved along body-frame axes, averaged over time interval.
16418 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
16419 * resolved along body-frame axes, averaged over time interval.
16420 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
16421 * resolved along body-frame axes, averaged over time interval.
16422 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
16423 * resolved along body-frame axes, averaged over time interval.
16424 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
16425 * resolved along body-frame axes, averaged over time interval.
16426 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
16427 * resolved along body-frame axes, averaged over time interval.
16428 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
16429 * @param result instance where new estimated NED frame containing new body position,
16430 * velocity and coordinate transformation matrix will be stored.
16431 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
16432 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
16433 * body-to-NED-frame coordinate transformation matrix are
16434 * invalid.
16435 */
16436 public static void navigateNED(
16437 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
16438 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
16439 final Acceleration fx, final Acceleration fy, final Acceleration fz,
16440 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
16441 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
16442 InvalidSourceAndDestinationFrameTypeException {
16443 navigateNED(convertTimeToDouble(timeInterval), oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity,
16444 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
16445 }
16446
16447 /**
16448 * Runs precision local-navigation-frame inertial navigation equations.
16449 * NOTE: only the attitude update and specific force frame transformation
16450 * phases are precise.
16451 *
16452 * @param timeInterval time interval between epochs expressed in seconds (s).
16453 * @param oldPosition previous curvilinear position expressed in terms of latitude,
16454 * longitude and height.
16455 * @param oldC previous body-to-NED coordinate transformation.
16456 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
16457 * along north, east and down axes.
16458 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
16459 * resolved along body-frame axes, averaged over time interval.
16460 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
16461 * resolved along body-frame axes, averaged over time interval.
16462 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
16463 * resolved along body-frame axes, averaged over time interval.
16464 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
16465 * resolved along body-frame axes, averaged over time interval.
16466 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
16467 * resolved along body-frame axes, averaged over time interval.
16468 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
16469 * resolved along body-frame axes, averaged over time interval.
16470 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
16471 * @param result instance where new estimated NED frame containing new body position,
16472 * velocity and coordinate transformation matrix will be stored.
16473 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
16474 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
16475 * body-to-NED-frame coordinate transformation matrix are
16476 * invalid.
16477 */
16478 public static void navigateNED(
16479 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
16480 final NEDVelocity oldVelocity, final Acceleration fx, final Acceleration fy, final Acceleration fz,
16481 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
16482 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
16483 InvalidSourceAndDestinationFrameTypeException {
16484 navigateNED(timeInterval, oldPosition, oldC, oldVelocity.getVn(), oldVelocity.getVe(), oldVelocity.getVd(),
16485 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
16486 }
16487
16488 /**
16489 * Runs precision local-navigation-frame inertial navigation equations.
16490 * NOTE: only the attitude update and specific force frame transformation
16491 * phases are precise.
16492 *
16493 * @param timeInterval time interval between epochs.
16494 * @param oldPosition previous curvilinear position expressed in terms of latitude,
16495 * longitude and height.
16496 * @param oldC previous body-to-NED coordinate transformation.
16497 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
16498 * along north, east and down axes.
16499 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
16500 * resolved along body-frame axes, averaged over time interval.
16501 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
16502 * resolved along body-frame axes, averaged over time interval.
16503 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
16504 * resolved along body-frame axes, averaged over time interval.
16505 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
16506 * resolved along body-frame axes, averaged over time interval.
16507 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
16508 * resolved along body-frame axes, averaged over time interval.
16509 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
16510 * resolved along body-frame axes, averaged over time interval.
16511 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
16512 * @param result instance where new estimated NED frame containing new body position,
16513 * velocity and coordinate transformation matrix will be stored.
16514 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
16515 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
16516 * body-to-NED-frame coordinate transformation matrix are
16517 * invalid.
16518 */
16519 public static void navigateNED(
16520 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
16521 final NEDVelocity oldVelocity, final Acceleration fx, final Acceleration fy, final Acceleration fz,
16522 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
16523 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
16524 InvalidSourceAndDestinationFrameTypeException {
16525 navigateNED(convertTimeToDouble(timeInterval), oldPosition, oldC, oldVelocity, fx, fy, fz,
16526 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
16527 }
16528
16529 /**
16530 * Runs precision local-navigation-frame inertial navigation equations.
16531 * NOTE: only the attitude update and specific force frame transformation
16532 * phases are precise.
16533 *
16534 * @param timeInterval time interval between epochs expressed in seconds (s).
16535 * @param oldLatitude previous latitude angle.
16536 * @param oldLongitude previous longitude angle.
16537 * @param oldHeight previous height.
16538 * @param oldC previous body-to-NED coordinate transformation.
16539 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
16540 * resolved along NED-frame axes and expressed in meters per second (m/s).
16541 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
16542 * resolved along NED-frame axes and expressed in meters per second (m/s).
16543 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
16544 * resolved along NED-frame axes and expressed in meters per second (m/s).
16545 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
16546 * resolved along body-frame axes, averaged over time interval.
16547 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
16548 * resolved along body-frame axes, averaged over time interval.
16549 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
16550 * resolved along body-frame axes, averaged over time interval.
16551 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
16552 * resolved along body-frame axes, averaged over time interval.
16553 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
16554 * resolved along body-frame axes, averaged over time interval.
16555 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
16556 * resolved along body-frame axes, averaged over time interval.
16557 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
16558 * @param result instance where new estimated NED frame containing new body position,
16559 * velocity and coordinate transformation matrix will be stored.
16560 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
16561 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
16562 * body-to-NED-frame coordinate transformation matrix are
16563 * invalid.
16564 */
16565 public static void navigateNED(
16566 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
16567 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
16568 final Acceleration fx, final Acceleration fy, final Acceleration fz,
16569 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
16570 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
16571 InvalidSourceAndDestinationFrameTypeException {
16572 navigateNED(timeInterval, convertAngleToDouble(oldLatitude), convertAngleToDouble(oldLongitude),
16573 convertDistanceToDouble(oldHeight), oldC, oldVn, oldVe, oldVd,
16574 convertAccelerationToDouble(fx), convertAccelerationToDouble(fy), convertAccelerationToDouble(fz),
16575 convertAngularSpeedToDouble(angularRateX), convertAngularSpeedToDouble(angularRateY),
16576 convertAngularSpeedToDouble(angularRateZ), accuracyThreshold, result);
16577 }
16578
16579 /**
16580 * Runs precision local-navigation-frame inertial navigation equations.
16581 * NOTE: only the attitude update and specific force frame transformation
16582 * phases are precise.
16583 *
16584 * @param timeInterval time interval between epochs.
16585 * @param oldLatitude previous latitude angle.
16586 * @param oldLongitude previous longitude angle.
16587 * @param oldHeight previous height.
16588 * @param oldC previous body-to-NED coordinate transformation.
16589 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
16590 * resolved along NED-frame axes and expressed in meters per second (m/s).
16591 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
16592 * resolved along NED-frame axes and expressed in meters per second (m/s).
16593 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
16594 * resolved along NED-frame axes and expressed in meters per second (m/s).
16595 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
16596 * resolved along body-frame axes, averaged over time interval.
16597 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
16598 * resolved along body-frame axes, averaged over time interval.
16599 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
16600 * resolved along body-frame axes, averaged over time interval.
16601 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
16602 * resolved along body-frame axes, averaged over time interval.
16603 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
16604 * resolved along body-frame axes, averaged over time interval.
16605 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
16606 * resolved along body-frame axes, averaged over time interval.
16607 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
16608 * @param result instance where new estimated NED frame containing new body position,
16609 * velocity and coordinate transformation matrix will be stored.
16610 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
16611 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
16612 * body-to-NED-frame coordinate transformation matrix are
16613 * invalid.
16614 */
16615 public static void navigateNED(
16616 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
16617 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
16618 final Acceleration fx, final Acceleration fy, final Acceleration fz,
16619 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
16620 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
16621 InvalidSourceAndDestinationFrameTypeException {
16622 navigateNED(convertTimeToDouble(timeInterval), oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
16623 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
16624 }
16625
16626 /**
16627 * Runs precision local-navigation-frame inertial navigation equations.
16628 * NOTE: only the attitude update and specific force frame transformation
16629 * phases are precise.
16630 *
16631 * @param timeInterval time interval between epochs expressed in seconds (s).
16632 * @param oldPosition previous curvilinear position expressed in terms of latitude,
16633 * longitude and height.
16634 * @param oldC previous body-to-NED coordinate transformation.
16635 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
16636 * resolved along NED-frame axes and expressed in meters per second (m/s).
16637 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
16638 * resolved along NED-frame axes and expressed in meters per second (m/s).
16639 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
16640 * resolved along NED-frame axes and expressed in meters per second (m/s).
16641 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
16642 * resolved along body-frame axes, averaged over time interval.
16643 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
16644 * resolved along body-frame axes, averaged over time interval.
16645 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
16646 * resolved along body-frame axes, averaged over time interval.
16647 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
16648 * resolved along body-frame axes, averaged over time interval.
16649 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
16650 * resolved along body-frame axes, averaged over time interval.
16651 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
16652 * resolved along body-frame axes, averaged over time interval.
16653 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
16654 * @param result instance where new estimated NED frame containing new body position,
16655 * velocity and coordinate transformation matrix will be stored.
16656 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
16657 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
16658 * body-to-NED-frame coordinate transformation matrix are
16659 * invalid.
16660 */
16661 public static void navigateNED(
16662 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
16663 final double oldVn, final double oldVe, final double oldVd,
16664 final Acceleration fx, final Acceleration fy, final Acceleration fz,
16665 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
16666 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
16667 InvalidSourceAndDestinationFrameTypeException {
16668 navigateNED(timeInterval, oldPosition.getLatitude(), oldPosition.getLongitude(), oldPosition.getHeight(), oldC,
16669 oldVn, oldVe, oldVd, fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
16670 }
16671
16672 /**
16673 * Runs precision local-navigation-frame inertial navigation equations.
16674 * NOTE: only the attitude update and specific force frame transformation
16675 * phases are precise.
16676 *
16677 * @param timeInterval time interval between epochs expressed in seconds (s).
16678 * @param oldPosition previous curvilinear position expressed in terms of latitude,
16679 * longitude and height.
16680 * @param oldC previous body-to-NED coordinate transformation.
16681 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
16682 * resolved along NED-frame axes and expressed in meters per second (m/s).
16683 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
16684 * resolved along NED-frame axes and expressed in meters per second (m/s).
16685 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
16686 * resolved along NED-frame axes and expressed in meters per second (m/s).
16687 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
16688 * resolved along body-frame axes, averaged over time interval.
16689 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
16690 * resolved along body-frame axes, averaged over time interval.
16691 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
16692 * resolved along body-frame axes, averaged over time interval.
16693 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
16694 * resolved along body-frame axes, averaged over time interval.
16695 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
16696 * resolved along body-frame axes, averaged over time interval.
16697 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
16698 * resolved along body-frame axes, averaged over time interval.
16699 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
16700 * @param result instance where new estimated NED frame containing new body position,
16701 * velocity and coordinate transformation matrix will be stored.
16702 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
16703 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
16704 * body-to-NED-frame coordinate transformation matrix are
16705 * invalid.
16706 */
16707 public static void navigateNED(
16708 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
16709 final double oldVn, final double oldVe, final double oldVd,
16710 final Acceleration fx, final Acceleration fy, final Acceleration fz,
16711 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
16712 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
16713 InvalidSourceAndDestinationFrameTypeException {
16714 navigateNED(convertTimeToDouble(timeInterval), oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
16715 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
16716 }
16717
16718 /**
16719 * Runs precision local-navigation-frame inertial navigation equations.
16720 * NOTE: only the attitude update and specific force frame transformation
16721 * phases are precise.
16722 *
16723 * @param timeInterval time interval between epochs expressed in seconds (s).
16724 * @param oldLatitude previous latitude expressed in radians (rad).
16725 * @param oldLongitude previous longitude expressed in radians (rad).
16726 * @param oldHeight previous height expressed in meters (m).
16727 * @param oldC previous body-to-NED coordinate transformation.
16728 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
16729 * resolved along NED-frame axes and expressed in meters per second (m/s).
16730 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
16731 * resolved along NED-frame axes and expressed in meters per second (m/s).
16732 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
16733 * resolved along NED-frame axes and expressed in meters per second (m/s).
16734 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
16735 * resolved along body-frame axes, averaged over time interval.
16736 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
16737 * resolved along body-frame axes, averaged over time interval.
16738 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
16739 * resolved along body-frame axes, averaged over time interval.
16740 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
16741 * resolved along body-frame axes, averaged over time interval.
16742 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
16743 * resolved along body-frame axes, averaged over time interval.
16744 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
16745 * resolved along body-frame axes, averaged over time interval.
16746 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
16747 * @param result instance where new estimated NED frame containing new body position,
16748 * velocity and coordinate transformation matrix will be stored.
16749 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
16750 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
16751 * body-to-NED-frame coordinate transformation matrix are
16752 * invalid.
16753 */
16754 public static void navigateNED(
16755 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
16756 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
16757 final Acceleration fx, final Acceleration fy, final Acceleration fz,
16758 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
16759 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
16760 InvalidSourceAndDestinationFrameTypeException {
16761 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
16762 convertAccelerationToDouble(fx), convertAccelerationToDouble(fy), convertAccelerationToDouble(fz),
16763 convertAngularSpeedToDouble(angularRateX), convertAngularSpeedToDouble(angularRateY),
16764 convertAngularSpeedToDouble(angularRateZ), accuracyThreshold, result);
16765 }
16766
16767 /**
16768 * Runs precision local-navigation-frame inertial navigation equations.
16769 * NOTE: only the attitude update and specific force frame transformation
16770 * phases are precise.
16771 *
16772 * @param timeInterval time interval between epochs.
16773 * @param oldLatitude previous latitude expressed in radians (rad).
16774 * @param oldLongitude previous longitude expressed in radians (rad).
16775 * @param oldHeight previous height expressed in meters (m).
16776 * @param oldC previous body-to-NED coordinate transformation.
16777 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
16778 * resolved along NED-frame axes and expressed in meters per second (m/s).
16779 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
16780 * resolved along NED-frame axes and expressed in meters per second (m/s).
16781 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
16782 * resolved along NED-frame axes and expressed in meters per second (m/s).
16783 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
16784 * resolved along body-frame axes, averaged over time interval.
16785 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
16786 * resolved along body-frame axes, averaged over time interval.
16787 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
16788 * resolved along body-frame axes, averaged over time interval.
16789 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
16790 * resolved along body-frame axes, averaged over time interval.
16791 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
16792 * resolved along body-frame axes, averaged over time interval.
16793 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
16794 * resolved along body-frame axes, averaged over time interval.
16795 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
16796 * @param result instance where new estimated NED frame containing new body position,
16797 * velocity and coordinate transformation matrix will be stored.
16798 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
16799 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
16800 * body-to-NED-frame coordinate transformation matrix are
16801 * invalid.
16802 */
16803 public static void navigateNED(
16804 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
16805 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
16806 final Acceleration fx, final Acceleration fy, final Acceleration fz,
16807 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
16808 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
16809 InvalidSourceAndDestinationFrameTypeException {
16810 navigateNED(convertTimeToDouble(timeInterval), oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
16811 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
16812 }
16813
16814 /**
16815 * Runs precision local-navigation-frame inertial navigation equations.
16816 * NOTE: only the attitude update and specific force frame transformation
16817 * phases are precise.
16818 *
16819 * @param timeInterval time interval between epochs expressed in seconds (s).
16820 * @param oldLatitude previous latitude angle.
16821 * @param oldLongitude previous longitude angle.
16822 * @param oldHeight previous height.
16823 * @param oldC previous body-to-NED coordinate transformation.
16824 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
16825 * resolved along NED-frame axes.
16826 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
16827 * resolved along NED-frame axes.
16828 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
16829 * resolved along NED-frame axes.
16830 * @param kinematics body kinematics containing specific forces and angular rates applied to
16831 * the body.
16832 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
16833 * @param result instance where new estimated NED frame containing new body position,
16834 * velocity and coordinate transformation matrix will be stored.
16835 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
16836 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
16837 * body-to-NED-frame coordinate transformation matrix are
16838 * invalid.
16839 */
16840 public static void navigateNED(
16841 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
16842 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
16843 final BodyKinematics kinematics, final double accuracyThreshold, final NEDFrame result)
16844 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
16845 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
16846 convertSpeedToDouble(oldSpeedN), convertSpeedToDouble(oldSpeedE), convertSpeedToDouble(oldSpeedD),
16847 kinematics, accuracyThreshold, result);
16848 }
16849
16850 /**
16851 * Runs precision local-navigation-frame inertial navigation equations.
16852 * NOTE: only the attitude update and specific force frame transformation
16853 * phases are precise.
16854 *
16855 * @param timeInterval time interval between epochs expressed in seconds (s).
16856 * @param oldLatitude previous latitude angle.
16857 * @param oldLongitude previous longitude angle.
16858 * @param oldHeight previous height.
16859 * @param oldC previous body-to-NED coordinate transformation.
16860 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
16861 * resolved along NED-frame axes.
16862 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
16863 * resolved along NED-frame axes.
16864 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
16865 * resolved along NED-frame axes.
16866 * @param kinematics body kinematics containing specific forces and angular rates applied to
16867 * the body.
16868 * @param result instance where new estimated NED frame containing new body position,
16869 * velocity and coordinate transformation matrix will be stored.
16870 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
16871 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
16872 * body-to-NED-frame coordinate transformation matrix are
16873 * invalid.
16874 */
16875 public static void navigateNED(
16876 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
16877 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
16878 final BodyKinematics kinematics, final NEDFrame result) throws InertialNavigatorException,
16879 InvalidSourceAndDestinationFrameTypeException {
16880 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
16881 kinematics, DEFAULT_ACCURACY_THRESHOLD, result);
16882 }
16883
16884 /**
16885 * Runs precision local-navigation-frame inertial navigation equations.
16886 * NOTE: only the attitude update and specific force frame transformation
16887 * phases are precise.
16888 *
16889 * @param timeInterval time interval between epochs.
16890 * @param oldLatitude previous latitude angle.
16891 * @param oldLongitude previous longitude angle.
16892 * @param oldHeight previous height.
16893 * @param oldC previous body-to-NED coordinate transformation.
16894 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
16895 * resolved along NED-frame axes.
16896 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
16897 * resolved along NED-frame axes.
16898 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
16899 * resolved along NED-frame axes.
16900 * @param kinematics body kinematics containing specific forces and angular rates applied to
16901 * the body.
16902 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
16903 * @param result instance where new estimated NED frame containing new body position,
16904 * velocity and coordinate transformation matrix will be stored.
16905 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
16906 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
16907 * body-to-NED-frame coordinate transformation matrix are
16908 * invalid.
16909 */
16910 public static void navigateNED(
16911 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
16912 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
16913 final BodyKinematics kinematics, final double accuracyThreshold, final NEDFrame result)
16914 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
16915 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
16916 convertSpeedToDouble(oldSpeedN), convertSpeedToDouble(oldSpeedE), convertSpeedToDouble(oldSpeedD),
16917 kinematics, accuracyThreshold, result);
16918 }
16919
16920 /**
16921 * Runs precision local-navigation-frame inertial navigation equations.
16922 * NOTE: only the attitude update and specific force frame transformation
16923 * phases are precise.
16924 *
16925 * @param timeInterval time interval between epochs.
16926 * @param oldLatitude previous latitude angle.
16927 * @param oldLongitude previous longitude angle.
16928 * @param oldHeight previous height.
16929 * @param oldC previous body-to-NED coordinate transformation.
16930 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
16931 * resolved along NED-frame axes.
16932 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
16933 * resolved along NED-frame axes.
16934 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
16935 * resolved along NED-frame axes.
16936 * @param kinematics body kinematics containing specific forces and angular rates applied to
16937 * the body.
16938 * @param result instance where new estimated NED frame containing new body position,
16939 * velocity and coordinate transformation matrix will be stored.
16940 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
16941 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
16942 * body-to-NED-frame coordinate transformation matrix are
16943 * invalid.
16944 */
16945 public static void navigateNED(
16946 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
16947 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
16948 final BodyKinematics kinematics, final NEDFrame result) throws InertialNavigatorException,
16949 InvalidSourceAndDestinationFrameTypeException {
16950 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
16951 kinematics, DEFAULT_ACCURACY_THRESHOLD, result);
16952 }
16953
16954 /**
16955 * Runs precision local-navigation-frame inertial navigation equations.
16956 * NOTE: only the attitude update and specific force frame transformation
16957 * phases are precise.
16958 *
16959 * @param timeInterval time interval between epochs expressed in seconds (s).
16960 * @param oldFrame previous NED frame containing body position, velocity and
16961 * coordinate transformation matrix.
16962 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
16963 * resolved along body-frame axes, averaged over time interval and
16964 * expressed in meters per squared second (m/s^2).
16965 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
16966 * resolved along body-frame axes, averaged over time interval and
16967 * expressed in meters per squared second (m/s^2).
16968 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
16969 * resolved along body-frame axes, averaged over time interval and
16970 * expressed in meters per squared second (m/s^2).
16971 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
16972 * resolved along body-frame axes, averaged over time interval and
16973 * expressed in radians per second (rad/s).
16974 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
16975 * resolved along body-frame axes, averaged over time interval and
16976 * expressed in radians per second (rad/s).
16977 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
16978 * resolved along body-frame axes, averaged over time interval and
16979 * expressed in radians per second (rad/s).
16980 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
16981 * @param result instance where new estimated NED frame containing new body position,
16982 * velocity and coordinate transformation matrix will be stored.
16983 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
16984 */
16985 public static void navigateNED(
16986 final double timeInterval, final NEDFrame oldFrame, final double fx, final double fy, final double fz,
16987 final double angularRateX, final double angularRateY, final double angularRateZ,
16988 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException {
16989 try {
16990 navigateNED(timeInterval, oldFrame.getLatitude(), oldFrame.getLongitude(), oldFrame.getHeight(),
16991 oldFrame.getCoordinateTransformation(), oldFrame.getVn(), oldFrame.getVe(), oldFrame.getVd(),
16992 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
16993 } catch (final InvalidSourceAndDestinationFrameTypeException ignore) {
16994 // never happens
16995 }
16996 }
16997
16998 /**
16999 * Runs precision local-navigation-frame inertial navigation equations.
17000 * NOTE: only the attitude update and specific force frame transformation
17001 * phases are precise.
17002 *
17003 * @param timeInterval time interval between epochs expressed in seconds (s).
17004 * @param oldFrame previous NED frame containing body position, velocity and
17005 * coordinate transformation matrix.
17006 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
17007 * resolved along body-frame axes, averaged over time interval and
17008 * expressed in meters per squared second (m/s^2).
17009 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
17010 * resolved along body-frame axes, averaged over time interval and
17011 * expressed in meters per squared second (m/s^2).
17012 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
17013 * resolved along body-frame axes, averaged over time interval and
17014 * expressed in meters per squared second (m/s^2).
17015 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
17016 * resolved along body-frame axes, averaged over time interval and
17017 * expressed in radians per second (rad/s).
17018 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
17019 * resolved along body-frame axes, averaged over time interval and
17020 * expressed in radians per second (rad/s).
17021 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
17022 * resolved along body-frame axes, averaged over time interval and
17023 * expressed in radians per second (rad/s).
17024 * @param result instance where new estimated NED frame containing new body position,
17025 * velocity and coordinate transformation matrix will be stored.
17026 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
17027 */
17028 public static void navigateNED(
17029 final double timeInterval, final NEDFrame oldFrame, final double fx, final double fy, final double fz,
17030 final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result)
17031 throws InertialNavigatorException {
17032 navigateNED(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
17033 DEFAULT_ACCURACY_THRESHOLD, result);
17034 }
17035
17036 /**
17037 * Runs precision local-navigation-frame inertial navigation equations.
17038 * NOTE: only the attitude update and specific force frame transformation
17039 * phases are precise.
17040 *
17041 * @param timeInterval time interval between epochs.
17042 * @param oldFrame previous NED frame containing body position, velocity and
17043 * coordinate transformation matrix.
17044 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
17045 * resolved along body-frame axes, averaged over time interval and
17046 * expressed in meters per squared second (m/s^2).
17047 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
17048 * resolved along body-frame axes, averaged over time interval and
17049 * expressed in meters per squared second (m/s^2).
17050 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
17051 * resolved along body-frame axes, averaged over time interval and
17052 * expressed in meters per squared second (m/s^2).
17053 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
17054 * resolved along body-frame axes, averaged over time interval and
17055 * expressed in radians per second (rad/s).
17056 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
17057 * resolved along body-frame axes, averaged over time interval and
17058 * expressed in radians per second (rad/s).
17059 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
17060 * resolved along body-frame axes, averaged over time interval and
17061 * expressed in radians per second (rad/s).
17062 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
17063 * @param result instance where new estimated NED frame containing new body position,
17064 * velocity and coordinate transformation matrix will be stored.
17065 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
17066 */
17067 public static void navigateNED(
17068 final Time timeInterval, final NEDFrame oldFrame, final double fx, final double fy, final double fz,
17069 final double angularRateX, final double angularRateY, final double angularRateZ,
17070 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException {
17071 navigateNED(convertTimeToDouble(timeInterval), oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
17072 accuracyThreshold, result);
17073 }
17074
17075 /**
17076 * Runs precision local-navigation-frame inertial navigation equations.
17077 * NOTE: only the attitude update and specific force frame transformation
17078 * phases are precise.
17079 *
17080 * @param timeInterval time interval between epochs.
17081 * @param oldFrame previous NED frame containing body position, velocity and
17082 * coordinate transformation matrix.
17083 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
17084 * resolved along body-frame axes, averaged over time interval and
17085 * expressed in meters per squared second (m/s^2).
17086 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
17087 * resolved along body-frame axes, averaged over time interval and
17088 * expressed in meters per squared second (m/s^2).
17089 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
17090 * resolved along body-frame axes, averaged over time interval and
17091 * expressed in meters per squared second (m/s^2).
17092 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
17093 * resolved along body-frame axes, averaged over time interval and
17094 * expressed in radians per second (rad/s).
17095 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
17096 * resolved along body-frame axes, averaged over time interval and
17097 * expressed in radians per second (rad/s).
17098 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
17099 * resolved along body-frame axes, averaged over time interval and
17100 * expressed in radians per second (rad/s).
17101 * @param result instance where new estimated NED frame containing new body position,
17102 * velocity and coordinate transformation matrix will be stored.
17103 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
17104 */
17105 public static void navigateNED(
17106 final Time timeInterval, final NEDFrame oldFrame, final double fx, final double fy, final double fz,
17107 final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result)
17108 throws InertialNavigatorException {
17109 navigateNED(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
17110 DEFAULT_ACCURACY_THRESHOLD, result);
17111 }
17112
17113 /**
17114 * Runs precision local-navigation-frame inertial navigation equations.
17115 * NOTE: only the attitude update and specific force frame transformation
17116 * phases are precise.
17117 *
17118 * @param timeInterval time interval between epochs expressed in seconds (s).
17119 * @param oldFrame previous NED frame containing body position, velocity and
17120 * coordinate transformation matrix.
17121 * @param kinematics body kinematics containing specific forces and angular rates applied to
17122 * the body.
17123 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
17124 * @param result instance where new estimated NED frame containing new body position,
17125 * velocity and coordinate transformation matrix will be stored.
17126 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
17127 */
17128 public static void navigateNED(
17129 final double timeInterval, final NEDFrame oldFrame, final BodyKinematics kinematics,
17130 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException {
17131 try {
17132 navigateNED(timeInterval, oldFrame.getLatitude(), oldFrame.getLongitude(), oldFrame.getHeight(),
17133 oldFrame.getCoordinateTransformation(), oldFrame.getVn(), oldFrame.getVe(), oldFrame.getVd(),
17134 kinematics, accuracyThreshold, result);
17135 } catch (final InvalidSourceAndDestinationFrameTypeException ignore) {
17136 // never happens
17137 }
17138 }
17139
17140 /**
17141 * Runs precision local-navigation-frame inertial navigation equations.
17142 * NOTE: only the attitude update and specific force frame transformation
17143 * phases are precise.
17144 *
17145 * @param timeInterval time interval between epochs expressed in seconds (s).
17146 * @param oldFrame previous NED frame containing body position, velocity and
17147 * coordinate transformation matrix.
17148 * @param kinematics body kinematics containing specific forces and angular rates applied to
17149 * the body.
17150 * @param result instance where new estimated NED frame containing new body position,
17151 * velocity and coordinate transformation matrix will be stored.
17152 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
17153 */
17154 public static void navigateNED(
17155 final double timeInterval, final NEDFrame oldFrame, final BodyKinematics kinematics, final NEDFrame result)
17156 throws InertialNavigatorException {
17157 navigateNED(timeInterval, oldFrame, kinematics, DEFAULT_ACCURACY_THRESHOLD, result);
17158 }
17159
17160 /**
17161 * Runs precision local-navigation-frame inertial navigation equations.
17162 * NOTE: only the attitude update and specific force frame transformation
17163 * phases are precise.
17164 *
17165 * @param timeInterval time interval between epochs.
17166 * @param oldFrame previous NED frame containing body position, velocity and
17167 * coordinate transformation matrix.
17168 * @param kinematics body kinematics containing specific forces and angular rates applied to
17169 * the body.
17170 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
17171 * @param result instance where new estimated NED frame containing new body position,
17172 * velocity and coordinate transformation matrix will be stored.
17173 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
17174 */
17175 public static void navigateNED(
17176 final Time timeInterval, final NEDFrame oldFrame, final BodyKinematics kinematics,
17177 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException {
17178 navigateNED(convertTimeToDouble(timeInterval), oldFrame, kinematics, accuracyThreshold, result);
17179 }
17180
17181 /**
17182 * Runs precision local-navigation-frame inertial navigation equations.
17183 * NOTE: only the attitude update and specific force frame transformation
17184 * phases are precise.
17185 *
17186 * @param timeInterval time interval between epochs.
17187 * @param oldFrame previous NED frame containing body position, velocity and
17188 * coordinate transformation matrix.
17189 * @param kinematics body kinematics containing specific forces and angular rates applied to
17190 * the body.
17191 * @param result instance where new estimated NED frame containing new body position,
17192 * velocity and coordinate transformation matrix will be stored.
17193 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
17194 */
17195 public static void navigateNED(
17196 final Time timeInterval, final NEDFrame oldFrame, final BodyKinematics kinematics, final NEDFrame result)
17197 throws InertialNavigatorException {
17198 navigateNED(timeInterval, oldFrame, kinematics, DEFAULT_ACCURACY_THRESHOLD, result);
17199 }
17200
17201 /**
17202 * Runs precision local-navigation-frame inertial navigation equations.
17203 * NOTE: only the attitude update and specific force frame transformation
17204 * phases are precise.
17205 *
17206 * @param timeInterval time interval between epochs expressed in seconds (s).
17207 * @param oldFrame previous NED frame containing body position, velocity and
17208 * coordinate transformation matrix.
17209 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
17210 * resolved along body-frame axes, averaged over time interval.
17211 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
17212 * resolved along body-frame axes, averaged over time interval.
17213 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
17214 * resolved along body-frame axes, averaged over time interval.
17215 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
17216 * resolved along body-frame axes, averaged over time interval and
17217 * expressed in radians per second (rad/s).
17218 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
17219 * resolved along body-frame axes, averaged over time interval and
17220 * expressed in radians per second (rad/s).
17221 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
17222 * resolved along body-frame axes, averaged over time interval and
17223 * expressed in radians per second (rad/s).
17224 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
17225 * @param result instance where new estimated NED frame containing new body position,
17226 * velocity and coordinate transformation matrix will be stored.
17227 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
17228 */
17229 public static void navigateNED(
17230 final double timeInterval, final NEDFrame oldFrame,
17231 final Acceleration fx, final Acceleration fy, final Acceleration fz,
17232 final double angularRateX, final double angularRateY, final double angularRateZ,
17233 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException {
17234 try {
17235 navigateNED(timeInterval, oldFrame.getLatitude(), oldFrame.getLongitude(), oldFrame.getHeight(),
17236 oldFrame.getCoordinateTransformation(), oldFrame.getVn(), oldFrame.getVe(), oldFrame.getVd(),
17237 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
17238 } catch (final InvalidSourceAndDestinationFrameTypeException ignore) {
17239 // never happens
17240 }
17241 }
17242
17243 /**
17244 * Runs precision local-navigation-frame inertial navigation equations.
17245 * NOTE: only the attitude update and specific force frame transformation
17246 * phases are precise.
17247 *
17248 * @param timeInterval time interval between epochs expressed in seconds (s).
17249 * @param oldFrame previous NED frame containing body position, velocity and
17250 * coordinate transformation matrix.
17251 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
17252 * resolved along body-frame axes, averaged over time interval.
17253 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
17254 * resolved along body-frame axes, averaged over time interval.
17255 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
17256 * resolved along body-frame axes, averaged over time interval.
17257 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
17258 * resolved along body-frame axes, averaged over time interval and
17259 * expressed in radians per second (rad/s).
17260 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
17261 * resolved along body-frame axes, averaged over time interval and
17262 * expressed in radians per second (rad/s).
17263 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
17264 * resolved along body-frame axes, averaged over time interval and
17265 * expressed in radians per second (rad/s).
17266 * @param result instance where new estimated NED frame containing new body position,
17267 * velocity and coordinate transformation matrix will be stored.
17268 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
17269 */
17270 public static void navigateNED(
17271 final double timeInterval, final NEDFrame oldFrame,
17272 final Acceleration fx, final Acceleration fy, final Acceleration fz,
17273 final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result)
17274 throws InertialNavigatorException {
17275 navigateNED(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
17276 DEFAULT_ACCURACY_THRESHOLD, result);
17277 }
17278
17279 /**
17280 * Runs precision local-navigation-frame inertial navigation equations.
17281 * NOTE: only the attitude update and specific force frame transformation
17282 * phases are precise.
17283 *
17284 * @param timeInterval time interval between epochs.
17285 * @param oldFrame previous NED frame containing body position, velocity and
17286 * coordinate transformation matrix.
17287 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
17288 * resolved along body-frame axes, averaged over time interval.
17289 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
17290 * resolved along body-frame axes, averaged over time interval.
17291 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
17292 * resolved along body-frame axes, averaged over time interval.
17293 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
17294 * resolved along body-frame axes, averaged over time interval and
17295 * expressed in radians per second (rad/s).
17296 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
17297 * resolved along body-frame axes, averaged over time interval and
17298 * expressed in radians per second (rad/s).
17299 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
17300 * resolved along body-frame axes, averaged over time interval and
17301 * expressed in radians per second (rad/s).
17302 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
17303 * @param result instance where new estimated NED frame containing new body position,
17304 * velocity and coordinate transformation matrix will be stored.
17305 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
17306 */
17307 public static void navigateNED(
17308 final Time timeInterval, final NEDFrame oldFrame,
17309 final Acceleration fx, final Acceleration fy, final Acceleration fz,
17310 final double angularRateX, final double angularRateY, final double angularRateZ,
17311 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException {
17312 navigateNED(convertTimeToDouble(timeInterval), oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
17313 accuracyThreshold, result);
17314 }
17315
17316 /**
17317 * Runs precision local-navigation-frame inertial navigation equations.
17318 * NOTE: only the attitude update and specific force frame transformation
17319 * phases are precise.
17320 *
17321 * @param timeInterval time interval between epochs.
17322 * @param oldFrame previous NED frame containing body position, velocity and
17323 * coordinate transformation matrix.
17324 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
17325 * resolved along body-frame axes, averaged over time interval.
17326 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
17327 * resolved along body-frame axes, averaged over time interval.
17328 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
17329 * resolved along body-frame axes, averaged over time interval.
17330 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
17331 * resolved along body-frame axes, averaged over time interval and
17332 * expressed in radians per second (rad/s).
17333 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
17334 * resolved along body-frame axes, averaged over time interval and
17335 * expressed in radians per second (rad/s).
17336 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
17337 * resolved along body-frame axes, averaged over time interval and
17338 * expressed in radians per second (rad/s).
17339 * @param result instance where new estimated NED frame containing new body position,
17340 * velocity and coordinate transformation matrix will be stored.
17341 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
17342 */
17343 public static void navigateNED(
17344 final Time timeInterval, final NEDFrame oldFrame,
17345 final Acceleration fx, final Acceleration fy, final Acceleration fz,
17346 final double angularRateX, final double angularRateY, final double angularRateZ,
17347 final NEDFrame result) throws InertialNavigatorException {
17348 navigateNED(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
17349 DEFAULT_ACCURACY_THRESHOLD, result);
17350 }
17351
17352 /**
17353 * Runs precision local-navigation-frame inertial navigation equations.
17354 * NOTE: only the attitude update and specific force frame transformation
17355 * phases are precise.
17356 *
17357 * @param timeInterval time interval between epochs expressed in seconds (s).
17358 * @param oldFrame previous NED frame containing body position, velocity and
17359 * coordinate transformation matrix.
17360 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
17361 * resolved along body-frame axes, averaged over time interval and
17362 * expressed in meters per squared second (m/s^2).
17363 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
17364 * resolved along body-frame axes, averaged over time interval and
17365 * expressed in meters per squared second (m/s^2).
17366 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
17367 * resolved along body-frame axes, averaged over time interval and
17368 * expressed in meters per squared second (m/s^2).
17369 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
17370 * resolved along body-frame axes, averaged over time interval.
17371 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
17372 * resolved along body-frame axes, averaged over time interval.
17373 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
17374 * resolved along body-frame axes, averaged over time interval.
17375 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
17376 * @param result instance where new estimated NED frame containing new body position,
17377 * velocity and coordinate transformation matrix will be stored.
17378 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
17379 */
17380 public static void navigateNED(
17381 final double timeInterval, final NEDFrame oldFrame,
17382 final double fx, final double fy, final double fz,
17383 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
17384 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException {
17385 try {
17386 navigateNED(timeInterval, oldFrame.getLatitude(), oldFrame.getLongitude(), oldFrame.getHeight(),
17387 oldFrame.getCoordinateTransformation(), oldFrame.getVn(), oldFrame.getVe(), oldFrame.getVd(),
17388 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
17389 } catch (final InvalidSourceAndDestinationFrameTypeException ignore) {
17390 // never happens
17391 }
17392 }
17393
17394 /**
17395 * Runs precision local-navigation-frame inertial navigation equations.
17396 * NOTE: only the attitude update and specific force frame transformation
17397 * phases are precise.
17398 *
17399 * @param timeInterval time interval between epochs expressed in seconds (s).
17400 * @param oldFrame previous NED frame containing body position, velocity and
17401 * coordinate transformation matrix.
17402 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
17403 * resolved along body-frame axes, averaged over time interval and
17404 * expressed in meters per squared second (m/s^2).
17405 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
17406 * resolved along body-frame axes, averaged over time interval and
17407 * expressed in meters per squared second (m/s^2).
17408 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
17409 * resolved along body-frame axes, averaged over time interval and
17410 * expressed in meters per squared second (m/s^2).
17411 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
17412 * resolved along body-frame axes, averaged over time interval.
17413 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
17414 * resolved along body-frame axes, averaged over time interval.
17415 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
17416 * resolved along body-frame axes, averaged over time interval.
17417 * @param result instance where new estimated NED frame containing new body position,
17418 * velocity and coordinate transformation matrix will be stored.
17419 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
17420 */
17421 public static void navigateNED(
17422 final double timeInterval, final NEDFrame oldFrame, final double fx, final double fy, final double fz,
17423 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
17424 final NEDFrame result) throws InertialNavigatorException {
17425 navigateNED(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
17426 DEFAULT_ACCURACY_THRESHOLD, result);
17427 }
17428
17429 /**
17430 * Runs precision local-navigation-frame inertial navigation equations.
17431 * NOTE: only the attitude update and specific force frame transformation
17432 * phases are precise.
17433 *
17434 * @param timeInterval time interval between epochs.
17435 * @param oldFrame previous NED frame containing body position, velocity and
17436 * coordinate transformation matrix.
17437 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
17438 * resolved along body-frame axes, averaged over time interval and
17439 * expressed in meters per squared second (m/s^2).
17440 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
17441 * resolved along body-frame axes, averaged over time interval and
17442 * expressed in meters per squared second (m/s^2).
17443 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
17444 * resolved along body-frame axes, averaged over time interval and
17445 * expressed in meters per squared second (m/s^2).
17446 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
17447 * resolved along body-frame axes, averaged over time interval.
17448 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
17449 * resolved along body-frame axes, averaged over time interval.
17450 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
17451 * resolved along body-frame axes, averaged over time interval.
17452 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
17453 * @param result instance where new estimated NED frame containing new body position,
17454 * velocity and coordinate transformation matrix will be stored.
17455 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
17456 */
17457 public static void navigateNED(
17458 final Time timeInterval, final NEDFrame oldFrame, final double fx, final double fy, final double fz,
17459 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
17460 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException {
17461 navigateNED(convertTimeToDouble(timeInterval), oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
17462 accuracyThreshold, result);
17463 }
17464
17465 /**
17466 * Runs precision local-navigation-frame inertial navigation equations.
17467 * NOTE: only the attitude update and specific force frame transformation
17468 * phases are precise.
17469 *
17470 * @param timeInterval time interval between epochs.
17471 * @param oldFrame previous NED frame containing body position, velocity and
17472 * coordinate transformation matrix.
17473 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
17474 * resolved along body-frame axes, averaged over time interval and
17475 * expressed in meters per squared second (m/s^2).
17476 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
17477 * resolved along body-frame axes, averaged over time interval and
17478 * expressed in meters per squared second (m/s^2).
17479 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
17480 * resolved along body-frame axes, averaged over time interval and
17481 * expressed in meters per squared second (m/s^2).
17482 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
17483 * resolved along body-frame axes, averaged over time interval.
17484 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
17485 * resolved along body-frame axes, averaged over time interval.
17486 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
17487 * resolved along body-frame axes, averaged over time interval.
17488 * @param result instance where new estimated NED frame containing new body position,
17489 * velocity and coordinate transformation matrix will be stored.
17490 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
17491 */
17492 public static void navigateNED(
17493 final Time timeInterval, final NEDFrame oldFrame, final double fx, final double fy, final double fz,
17494 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
17495 final NEDFrame result) throws InertialNavigatorException {
17496 navigateNED(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
17497 DEFAULT_ACCURACY_THRESHOLD, result);
17498 }
17499
17500 /**
17501 * Runs precision local-navigation-frame inertial navigation equations.
17502 * NOTE: only the attitude update and specific force frame transformation
17503 * phases are precise.
17504 *
17505 * @param timeInterval time interval between epochs expressed in seconds (s).
17506 * @param oldFrame previous NED frame containing body position, velocity and
17507 * coordinate transformation matrix.
17508 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
17509 * resolved along body-frame axes, averaged over time interval and
17510 * expressed in meters per squared second (m/s^2).
17511 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
17512 * resolved along body-frame axes, averaged over time interval and
17513 * expressed in meters per squared second (m/s^2).
17514 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
17515 * resolved along body-frame axes, averaged over time interval and
17516 * expressed in meters per squared second (m/s^2).
17517 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
17518 * resolved along body-frame axes, averaged over time interval.
17519 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
17520 * resolved along body-frame axes, averaged over time interval.
17521 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
17522 * resolved along body-frame axes, averaged over time interval.
17523 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
17524 * @param result instance where new estimated NED frame containing new body position,
17525 * velocity and coordinate transformation matrix will be stored.
17526 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
17527 */
17528 public static void navigateNED(
17529 final double timeInterval, final NEDFrame oldFrame,
17530 final Acceleration fx, final Acceleration fy, final Acceleration fz,
17531 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
17532 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException {
17533 try {
17534 navigateNED(timeInterval, oldFrame.getLatitude(), oldFrame.getLongitude(), oldFrame.getHeight(),
17535 oldFrame.getCoordinateTransformation(), oldFrame.getVn(), oldFrame.getVe(), oldFrame.getVd(),
17536 convertAccelerationToDouble(fx), convertAccelerationToDouble(fy), convertAccelerationToDouble(fz),
17537 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
17538 } catch (final InvalidSourceAndDestinationFrameTypeException ignore) {
17539 // never happens
17540 }
17541 }
17542
17543 /**
17544 * Runs precision local-navigation-frame inertial navigation equations.
17545 * NOTE: only the attitude update and specific force frame transformation
17546 * phases are precise.
17547 *
17548 * @param timeInterval time interval between epochs expressed in seconds (s).
17549 * @param oldFrame previous NED frame containing body position, velocity and
17550 * coordinate transformation matrix.
17551 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
17552 * resolved along body-frame axes, averaged over time interval and
17553 * expressed in meters per squared second (m/s^2).
17554 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
17555 * resolved along body-frame axes, averaged over time interval and
17556 * expressed in meters per squared second (m/s^2).
17557 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
17558 * resolved along body-frame axes, averaged over time interval and
17559 * expressed in meters per squared second (m/s^2).
17560 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
17561 * resolved along body-frame axes, averaged over time interval.
17562 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
17563 * resolved along body-frame axes, averaged over time interval.
17564 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
17565 * resolved along body-frame axes, averaged over time interval.
17566 * @param result instance where new estimated NED frame containing new body position,
17567 * velocity and coordinate transformation matrix will be stored.
17568 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
17569 */
17570 public static void navigateNED(
17571 final double timeInterval, final NEDFrame oldFrame,
17572 final Acceleration fx, final Acceleration fy, final Acceleration fz,
17573 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
17574 final NEDFrame result) throws InertialNavigatorException {
17575 navigateNED(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
17576 DEFAULT_ACCURACY_THRESHOLD, result);
17577 }
17578
17579 /**
17580 * Runs precision local-navigation-frame inertial navigation equations.
17581 * NOTE: only the attitude update and specific force frame transformation
17582 * phases are precise.
17583 *
17584 * @param timeInterval time interval between epochs expressed in seconds (s).
17585 * @param oldFrame previous NED frame containing body position, velocity and
17586 * coordinate transformation matrix.
17587 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
17588 * resolved along body-frame axes, averaged over time interval and
17589 * expressed in meters per squared second (m/s^2).
17590 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
17591 * resolved along body-frame axes, averaged over time interval and
17592 * expressed in meters per squared second (m/s^2).
17593 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
17594 * resolved along body-frame axes, averaged over time interval and
17595 * expressed in meters per squared second (m/s^2).
17596 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
17597 * resolved along body-frame axes, averaged over time interval.
17598 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
17599 * resolved along body-frame axes, averaged over time interval.
17600 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
17601 * resolved along body-frame axes, averaged over time interval.
17602 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
17603 * @param result instance where new estimated NED frame containing new body position,
17604 * velocity and coordinate transformation matrix will be stored.
17605 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
17606 */
17607 public static void navigateNED(
17608 final Time timeInterval, final NEDFrame oldFrame,
17609 final Acceleration fx, final Acceleration fy, final Acceleration fz,
17610 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
17611 final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException {
17612 navigateNED(convertTimeToDouble(timeInterval), oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
17613 accuracyThreshold, result);
17614 }
17615
17616 /**
17617 * Runs precision local-navigation-frame inertial navigation equations.
17618 * NOTE: only the attitude update and specific force frame transformation
17619 * phases are precise.
17620 *
17621 * @param timeInterval time interval between epochs expressed in seconds (s).
17622 * @param oldFrame previous NED frame containing body position, velocity and
17623 * coordinate transformation matrix.
17624 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
17625 * resolved along body-frame axes, averaged over time interval and
17626 * expressed in meters per squared second (m/s^2).
17627 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
17628 * resolved along body-frame axes, averaged over time interval and
17629 * expressed in meters per squared second (m/s^2).
17630 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
17631 * resolved along body-frame axes, averaged over time interval and
17632 * expressed in meters per squared second (m/s^2).
17633 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
17634 * resolved along body-frame axes, averaged over time interval.
17635 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
17636 * resolved along body-frame axes, averaged over time interval.
17637 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
17638 * resolved along body-frame axes, averaged over time interval.
17639 * @param result instance where new estimated NED frame containing new body position,
17640 * velocity and coordinate transformation matrix will be stored.
17641 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
17642 */
17643 public static void navigateNED(
17644 final Time timeInterval, final NEDFrame oldFrame,
17645 final Acceleration fx, final Acceleration fy, final Acceleration fz,
17646 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
17647 final NEDFrame result) throws InertialNavigatorException {
17648 navigateNED(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
17649 DEFAULT_ACCURACY_THRESHOLD, result);
17650 }
17651
17652 /**
17653 * Runs precision local-navigation-frame inertial navigation equations.
17654 * NOTE: only the attitude update and specific force frame transformation
17655 * phases are precise.
17656 *
17657 * @param timeInterval time interval between epochs expressed in seconds (s).
17658 * @param oldLatitude previous latitude expressed in radians (rad).
17659 * @param oldLongitude previous longitude expressed in radians (rad).
17660 * @param oldHeight previous height expressed in meters (m).
17661 * @param oldC previous body-to-NED coordinate transformation.
17662 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
17663 * resolved along NED-frame axes and expressed in meters per second (m/s).
17664 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
17665 * resolved along NED-frame axes and expressed in meters per second (m/s).
17666 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
17667 * resolved along NED-frame axes and expressed in meters per second (m/s).
17668 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
17669 * resolved along body-frame axes, averaged over time interval and
17670 * expressed in meters per squared second (m/s^2).
17671 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
17672 * resolved along body-frame axes, averaged over time interval and
17673 * expressed in meters per squared second (m/s^2).
17674 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
17675 * resolved along body-frame axes, averaged over time interval and
17676 * expressed in meters per squared second (m/s^2).
17677 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
17678 * resolved along body-frame axes, averaged over time interval and
17679 * expressed in radians per second (rad/s).
17680 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
17681 * resolved along body-frame axes, averaged over time interval and
17682 * expressed in radians per second (rad/s).
17683 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
17684 * resolved along body-frame axes, averaged over time interval and
17685 * expressed in radians per second (rad/s).
17686 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
17687 * @return estimated NED frame containing new body position, velocity and coordinate
17688 * transformation matrix.
17689 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
17690 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
17691 * body-to-NED-frame coordinate transformation matrix are
17692 * invalid.
17693 */
17694 public static NEDFrame navigateNEDAndReturnNew(
17695 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
17696 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
17697 final double fx, final double fy, final double fz,
17698 final double angularRateX, final double angularRateY, final double angularRateZ,
17699 final double accuracyThreshold) throws InertialNavigatorException,
17700 InvalidSourceAndDestinationFrameTypeException {
17701 final var result = new NEDFrame();
17702 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
17703 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
17704 return result;
17705 }
17706
17707 /**
17708 * Runs precision local-navigation-frame inertial navigation equations.
17709 * NOTE: only the attitude update and specific force frame transformation
17710 * phases are precise.
17711 *
17712 * @param timeInterval time interval between epochs.
17713 * @param oldLatitude previous latitude expressed in radians (rad).
17714 * @param oldLongitude previous longitude expressed in radians (rad).
17715 * @param oldHeight previous height expressed in meters (m).
17716 * @param oldC previous body-to-NED coordinate transformation.
17717 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
17718 * resolved along NED-frame axes and expressed in meters per second (m/s).
17719 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
17720 * resolved along NED-frame axes and expressed in meters per second (m/s).
17721 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
17722 * resolved along NED-frame axes and expressed in meters per second (m/s).
17723 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
17724 * resolved along body-frame axes, averaged over time interval and
17725 * expressed in meters per squared second (m/s^2).
17726 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
17727 * resolved along body-frame axes, averaged over time interval and
17728 * expressed in meters per squared second (m/s^2).
17729 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
17730 * resolved along body-frame axes, averaged over time interval and
17731 * expressed in meters per squared second (m/s^2).
17732 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
17733 * resolved along body-frame axes, averaged over time interval and
17734 * expressed in radians per second (rad/s).
17735 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
17736 * resolved along body-frame axes, averaged over time interval and
17737 * expressed in radians per second (rad/s).
17738 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
17739 * resolved along body-frame axes, averaged over time interval and
17740 * expressed in radians per second (rad/s).
17741 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
17742 * @return estimated NED frame containing new body position, velocity and coordinate
17743 * transformation matrix.
17744 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
17745 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
17746 * body-to-NED-frame coordinate transformation matrix are
17747 * invalid.
17748 */
17749 public static NEDFrame navigateNEDAndReturnNew(
17750 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
17751 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
17752 final double fx, final double fy, final double fz,
17753 final double angularRateX, final double angularRateY, final double angularRateZ,
17754 final double accuracyThreshold) throws InertialNavigatorException,
17755 InvalidSourceAndDestinationFrameTypeException {
17756 final var result = new NEDFrame();
17757 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
17758 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
17759 return result;
17760 }
17761
17762 /**
17763 * Runs precision local-navigation-frame inertial navigation equations.
17764 * NOTE: only the attitude update and specific force frame transformation
17765 * phases are precise.
17766 *
17767 * @param timeInterval time interval between epochs expressed in seconds (s).
17768 * @param oldPosition previous curvilinear position expressed in terms of latitude,
17769 * longitude and height.
17770 * @param oldC previous body-to-NED coordinate transformation.
17771 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
17772 * resolved along NED-frame axes and expressed in meters per second (m/s).
17773 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
17774 * resolved along NED-frame axes and expressed in meters per second (m/s).
17775 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
17776 * resolved along NED-frame axes and expressed in meters per second (m/s).
17777 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
17778 * resolved along body-frame axes, averaged over time interval and
17779 * expressed in meters per squared second (m/s^2).
17780 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
17781 * resolved along body-frame axes, averaged over time interval and
17782 * expressed in meters per squared second (m/s^2).
17783 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
17784 * resolved along body-frame axes, averaged over time interval and
17785 * expressed in meters per squared second (m/s^2).
17786 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
17787 * resolved along body-frame axes, averaged over time interval and
17788 * expressed in radians per second (rad/s).
17789 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
17790 * resolved along body-frame axes, averaged over time interval and
17791 * expressed in radians per second (rad/s).
17792 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
17793 * resolved along body-frame axes, averaged over time interval and
17794 * expressed in radians per second (rad/s).
17795 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
17796 * @return estimated NED frame containing new body position, velocity and coordinate
17797 * transformation matrix.
17798 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
17799 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
17800 * body-to-NED-frame coordinate transformation matrix are
17801 * invalid.
17802 */
17803 public static NEDFrame navigateNEDAndReturnNew(
17804 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
17805 final double oldVn, final double oldVe, final double oldVd,
17806 final double fx, final double fy, final double fz,
17807 final double angularRateX, final double angularRateY, final double angularRateZ,
17808 final double accuracyThreshold) throws InertialNavigatorException,
17809 InvalidSourceAndDestinationFrameTypeException {
17810 final var result = new NEDFrame();
17811 navigateNED(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
17812 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
17813 return result;
17814 }
17815
17816 /**
17817 * Runs precision local-navigation-frame inertial navigation equations.
17818 * NOTE: only the attitude update and specific force frame transformation
17819 * phases are precise.
17820 *
17821 * @param timeInterval time interval between epochs.
17822 * @param oldPosition previous curvilinear position expressed in terms of latitude,
17823 * longitude and height.
17824 * @param oldC previous body-to-NED coordinate transformation.
17825 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
17826 * resolved along NED-frame axes and expressed in meters per second (m/s).
17827 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
17828 * resolved along NED-frame axes and expressed in meters per second (m/s).
17829 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
17830 * resolved along NED-frame axes and expressed in meters per second (m/s).
17831 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
17832 * resolved along body-frame axes, averaged over time interval and
17833 * expressed in meters per squared second (m/s^2).
17834 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
17835 * resolved along body-frame axes, averaged over time interval and
17836 * expressed in meters per squared second (m/s^2).
17837 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
17838 * resolved along body-frame axes, averaged over time interval and
17839 * expressed in meters per squared second (m/s^2).
17840 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
17841 * resolved along body-frame axes, averaged over time interval and
17842 * expressed in radians per second (rad/s).
17843 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
17844 * resolved along body-frame axes, averaged over time interval and
17845 * expressed in radians per second (rad/s).
17846 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
17847 * resolved along body-frame axes, averaged over time interval and
17848 * expressed in radians per second (rad/s).
17849 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
17850 * @return estimated NED frame containing new body position, velocity and coordinate
17851 * transformation matrix.
17852 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
17853 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
17854 * body-to-NED-frame coordinate transformation matrix are
17855 * invalid.
17856 */
17857 public static NEDFrame navigateNEDAndReturnNew(
17858 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
17859 final double oldVn, final double oldVe, final double oldVd,
17860 final double fx, final double fy, final double fz,
17861 final double angularRateX, final double angularRateY, final double angularRateZ,
17862 final double accuracyThreshold) throws InertialNavigatorException,
17863 InvalidSourceAndDestinationFrameTypeException {
17864 final var result = new NEDFrame();
17865 navigateNED(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
17866 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
17867 return result;
17868 }
17869
17870 /**
17871 * Runs precision local-navigation-frame inertial navigation equations.
17872 * NOTE: only the attitude update and specific force frame transformation
17873 * phases are precise.
17874 *
17875 * @param timeInterval time interval between epochs expressed in seconds (s).
17876 * @param oldLatitude previous latitude expressed in radians (rad).
17877 * @param oldLongitude previous longitude expressed in radians (rad).
17878 * @param oldHeight previous height expressed in meters (m).
17879 * @param oldC previous body-to-NED coordinate transformation.
17880 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
17881 * along north, east and down axes.
17882 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
17883 * resolved along body-frame axes, averaged over time interval and
17884 * expressed in meters per squared second (m/s^2).
17885 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
17886 * resolved along body-frame axes, averaged over time interval and
17887 * expressed in meters per squared second (m/s^2).
17888 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
17889 * resolved along body-frame axes, averaged over time interval and
17890 * expressed in meters per squared second (m/s^2).
17891 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
17892 * resolved along body-frame axes, averaged over time interval and
17893 * expressed in radians per second (rad/s).
17894 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
17895 * resolved along body-frame axes, averaged over time interval and
17896 * expressed in radians per second (rad/s).
17897 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
17898 * resolved along body-frame axes, averaged over time interval and
17899 * expressed in radians per second (rad/s).
17900 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
17901 * @return estimated NED frame containing new body position, velocity and coordinate
17902 * transformation matrix.
17903 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
17904 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
17905 * body-to-NED-frame coordinate transformation matrix are
17906 * invalid.
17907 */
17908 public static NEDFrame navigateNEDAndReturnNew(
17909 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
17910 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
17911 final double fx, final double fy, final double fz,
17912 final double angularRateX, final double angularRateY, final double angularRateZ,
17913 final double accuracyThreshold) throws InertialNavigatorException,
17914 InvalidSourceAndDestinationFrameTypeException {
17915 final var result = new NEDFrame();
17916 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
17917 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
17918 return result;
17919 }
17920
17921 /**
17922 * Runs precision local-navigation-frame inertial navigation equations.
17923 * NOTE: only the attitude update and specific force frame transformation
17924 * phases are precise.
17925 *
17926 * @param timeInterval time interval between epochs.
17927 * @param oldLatitude previous latitude expressed in radians (rad).
17928 * @param oldLongitude previous longitude expressed in radians (rad).
17929 * @param oldHeight previous height expressed in meters (m).
17930 * @param oldC previous body-to-NED coordinate transformation.
17931 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
17932 * along north, east and down axes.
17933 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
17934 * resolved along body-frame axes, averaged over time interval and
17935 * expressed in meters per squared second (m/s^2).
17936 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
17937 * resolved along body-frame axes, averaged over time interval and
17938 * expressed in meters per squared second (m/s^2).
17939 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
17940 * resolved along body-frame axes, averaged over time interval and
17941 * expressed in meters per squared second (m/s^2).
17942 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
17943 * resolved along body-frame axes, averaged over time interval and
17944 * expressed in radians per second (rad/s).
17945 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
17946 * resolved along body-frame axes, averaged over time interval and
17947 * expressed in radians per second (rad/s).
17948 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
17949 * resolved along body-frame axes, averaged over time interval and
17950 * expressed in radians per second (rad/s).
17951 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
17952 * @return estimated NED frame containing new body position, velocity and coordinate
17953 * transformation matrix.
17954 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
17955 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
17956 * body-to-NED-frame coordinate transformation matrix are
17957 * invalid.
17958 */
17959 public static NEDFrame navigateNEDAndReturnNew(
17960 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
17961 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
17962 final double fx, final double fy, final double fz,
17963 final double angularRateX, final double angularRateY, final double angularRateZ,
17964 final double accuracyThreshold) throws InertialNavigatorException,
17965 InvalidSourceAndDestinationFrameTypeException {
17966 final var result = new NEDFrame();
17967 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
17968 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
17969 return result;
17970 }
17971
17972 /**
17973 * Runs precision local-navigation-frame inertial navigation equations.
17974 * NOTE: only the attitude update and specific force frame transformation
17975 * phases are precise.
17976 *
17977 * @param timeInterval time interval between epochs expressed in seconds (s).
17978 * @param oldPosition previous curvilinear position expressed in terms of latitude,
17979 * longitude and height.
17980 * @param oldC previous body-to-NED coordinate transformation.
17981 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
17982 * along north, east and down axes.
17983 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
17984 * resolved along body-frame axes, averaged over time interval and
17985 * expressed in meters per squared second (m/s^2).
17986 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
17987 * resolved along body-frame axes, averaged over time interval and
17988 * expressed in meters per squared second (m/s^2).
17989 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
17990 * resolved along body-frame axes, averaged over time interval and
17991 * expressed in meters per squared second (m/s^2).
17992 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
17993 * resolved along body-frame axes, averaged over time interval and
17994 * expressed in radians per second (rad/s).
17995 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
17996 * resolved along body-frame axes, averaged over time interval and
17997 * expressed in radians per second (rad/s).
17998 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
17999 * resolved along body-frame axes, averaged over time interval and
18000 * expressed in radians per second (rad/s).
18001 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
18002 * @return estimated NED frame containing new body position, velocity and coordinate
18003 * transformation matrix.
18004 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
18005 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
18006 * body-to-NED-frame coordinate transformation matrix are
18007 * invalid.
18008 */
18009 public static NEDFrame navigateNEDAndReturnNew(
18010 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
18011 final NEDVelocity oldVelocity, final double fx, final double fy, final double fz,
18012 final double angularRateX, final double angularRateY, final double angularRateZ,
18013 final double accuracyThreshold) throws InertialNavigatorException,
18014 InvalidSourceAndDestinationFrameTypeException {
18015 final var result = new NEDFrame();
18016 navigateNED(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
18017 accuracyThreshold, result);
18018 return result;
18019 }
18020
18021 /**
18022 * Runs precision local-navigation-frame inertial navigation equations.
18023 * NOTE: only the attitude update and specific force frame transformation
18024 * phases are precise.
18025 *
18026 * @param timeInterval time interval between epochs expressed in seconds (s).
18027 * @param oldPosition previous curvilinear position expressed in terms of latitude,
18028 * longitude and height.
18029 * @param oldC previous body-to-NED coordinate transformation.
18030 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
18031 * along north, east and down axes.
18032 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
18033 * resolved along body-frame axes, averaged over time interval and
18034 * expressed in meters per squared second (m/s^2).
18035 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
18036 * resolved along body-frame axes, averaged over time interval and
18037 * expressed in meters per squared second (m/s^2).
18038 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
18039 * resolved along body-frame axes, averaged over time interval and
18040 * expressed in meters per squared second (m/s^2).
18041 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
18042 * resolved along body-frame axes, averaged over time interval and
18043 * expressed in radians per second (rad/s).
18044 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
18045 * resolved along body-frame axes, averaged over time interval and
18046 * expressed in radians per second (rad/s).
18047 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
18048 * resolved along body-frame axes, averaged over time interval and
18049 * expressed in radians per second (rad/s).
18050 * @return estimated NED frame containing new body position, velocity and coordinate
18051 * transformation matrix.
18052 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
18053 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
18054 * body-to-NED-frame coordinate transformation matrix are
18055 * invalid.
18056 */
18057 public static NEDFrame navigateNEDAndReturnNew(
18058 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
18059 final NEDVelocity oldVelocity, final double fx, final double fy, final double fz,
18060 final double angularRateX, final double angularRateY, final double angularRateZ)
18061 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
18062 return navigateNEDAndReturnNew(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz,
18063 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
18064 }
18065
18066 /**
18067 * Runs precision local-navigation-frame inertial navigation equations.
18068 * NOTE: only the attitude update and specific force frame transformation
18069 * phases are precise.
18070 *
18071 * @param timeInterval time interval between epochs.
18072 * @param oldPosition previous curvilinear position expressed in terms of latitude,
18073 * longitude and height.
18074 * @param oldC previous body-to-NED coordinate transformation.
18075 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
18076 * along north, east and down axes.
18077 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
18078 * resolved along body-frame axes, averaged over time interval and
18079 * expressed in meters per squared second (m/s^2).
18080 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
18081 * resolved along body-frame axes, averaged over time interval and
18082 * expressed in meters per squared second (m/s^2).
18083 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
18084 * resolved along body-frame axes, averaged over time interval and
18085 * expressed in meters per squared second (m/s^2).
18086 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
18087 * resolved along body-frame axes, averaged over time interval and
18088 * expressed in radians per second (rad/s).
18089 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
18090 * resolved along body-frame axes, averaged over time interval and
18091 * expressed in radians per second (rad/s).
18092 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
18093 * resolved along body-frame axes, averaged over time interval and
18094 * expressed in radians per second (rad/s).
18095 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
18096 * @return estimated NED frame containing new body position, velocity and coordinate
18097 * transformation matrix.
18098 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
18099 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
18100 * body-to-NED-frame coordinate transformation matrix are
18101 * invalid.
18102 */
18103 public static NEDFrame navigateNEDAndReturnNew(
18104 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
18105 final NEDVelocity oldVelocity, final double fx, final double fy, final double fz,
18106 final double angularRateX, final double angularRateY, final double angularRateZ,
18107 final double accuracyThreshold) throws InertialNavigatorException,
18108 InvalidSourceAndDestinationFrameTypeException {
18109 final var result = new NEDFrame();
18110 navigateNED(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
18111 accuracyThreshold, result);
18112 return result;
18113 }
18114
18115 /**
18116 * Runs precision local-navigation-frame inertial navigation equations.
18117 * NOTE: only the attitude update and specific force frame transformation
18118 * phases are precise.
18119 *
18120 * @param timeInterval time interval between epochs.
18121 * @param oldPosition previous curvilinear position expressed in terms of latitude,
18122 * longitude and height.
18123 * @param oldC previous body-to-NED coordinate transformation.
18124 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
18125 * along north, east and down axes.
18126 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
18127 * resolved along body-frame axes, averaged over time interval and
18128 * expressed in meters per squared second (m/s^2).
18129 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
18130 * resolved along body-frame axes, averaged over time interval and
18131 * expressed in meters per squared second (m/s^2).
18132 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
18133 * resolved along body-frame axes, averaged over time interval and
18134 * expressed in meters per squared second (m/s^2).
18135 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
18136 * resolved along body-frame axes, averaged over time interval and
18137 * expressed in radians per second (rad/s).
18138 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
18139 * resolved along body-frame axes, averaged over time interval and
18140 * expressed in radians per second (rad/s).
18141 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
18142 * resolved along body-frame axes, averaged over time interval and
18143 * expressed in radians per second (rad/s).
18144 * @return estimated NED frame containing new body position, velocity and coordinate
18145 * transformation matrix.
18146 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
18147 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
18148 * body-to-NED-frame coordinate transformation matrix are
18149 * invalid.
18150 */
18151 public static NEDFrame navigateNEDAndReturnNew(
18152 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
18153 final NEDVelocity oldVelocity, final double fx, final double fy, final double fz,
18154 final double angularRateX, final double angularRateY, final double angularRateZ)
18155 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
18156 return navigateNEDAndReturnNew(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz,
18157 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
18158 }
18159
18160 /**
18161 * Runs precision local-navigation-frame inertial navigation equations.
18162 * NOTE: only the attitude update and specific force frame transformation
18163 * phases are precise.
18164 *
18165 * @param timeInterval time interval between epochs expressed in seconds (s).
18166 * @param oldLatitude previous latitude expressed in radians (rad).
18167 * @param oldLongitude previous longitude expressed in radians (rad).
18168 * @param oldHeight previous height expressed in meters (m).
18169 * @param oldC previous body-to-NED coordinate transformation.
18170 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
18171 * resolved along NED-frame axes and expressed in meters per second (m/s).
18172 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
18173 * resolved along NED-frame axes and expressed in meters per second (m/s).
18174 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
18175 * resolved along NED-frame axes and expressed in meters per second (m/s).
18176 * @param kinematics body kinematics containing specific forces and angular rates applied to
18177 * the body.
18178 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
18179 * @return estimated NED frame containing new body position, velocity and coordinate
18180 * transformation matrix.
18181 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
18182 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
18183 * body-to-NED-frame coordinate transformation matrix are
18184 * invalid.
18185 */
18186 public static NEDFrame navigateNEDAndReturnNew(
18187 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
18188 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
18189 final BodyKinematics kinematics, final double accuracyThreshold) throws InertialNavigatorException,
18190 InvalidSourceAndDestinationFrameTypeException {
18191 final var result = new NEDFrame();
18192 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, kinematics,
18193 accuracyThreshold, result);
18194 return result;
18195 }
18196
18197 /**
18198 * Runs precision local-navigation-frame inertial navigation equations.
18199 * NOTE: only the attitude update and specific force frame transformation
18200 * phases are precise.
18201 *
18202 * @param timeInterval time interval between epochs expressed in seconds (s).
18203 * @param oldLatitude previous latitude expressed in radians (rad).
18204 * @param oldLongitude previous longitude expressed in radians (rad).
18205 * @param oldHeight previous height expressed in meters (m).
18206 * @param oldC previous body-to-NED coordinate transformation.
18207 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
18208 * resolved along NED-frame axes and expressed in meters per second (m/s).
18209 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
18210 * resolved along NED-frame axes and expressed in meters per second (m/s).
18211 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
18212 * resolved along NED-frame axes and expressed in meters per second (m/s).
18213 * @param kinematics body kinematics containing specific forces and angular rates applied to
18214 * the body.
18215 * @return estimated NED frame containing new body position, velocity and coordinate
18216 * transformation matrix.
18217 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
18218 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
18219 * body-to-NED-frame coordinate transformation matrix are
18220 * invalid.
18221 */
18222 public static NEDFrame navigateNEDAndReturnNew(
18223 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
18224 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
18225 final BodyKinematics kinematics) throws InertialNavigatorException,
18226 InvalidSourceAndDestinationFrameTypeException {
18227 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
18228 kinematics, DEFAULT_ACCURACY_THRESHOLD);
18229 }
18230
18231 /**
18232 * Runs precision local-navigation-frame inertial navigation equations.
18233 * NOTE: only the attitude update and specific force frame transformation
18234 * phases are precise.
18235 *
18236 * @param timeInterval time interval between epochs.
18237 * @param oldLatitude previous latitude expressed in radians (rad).
18238 * @param oldLongitude previous longitude expressed in radians (rad).
18239 * @param oldHeight previous height expressed in meters (m).
18240 * @param oldC previous body-to-NED coordinate transformation.
18241 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
18242 * resolved along NED-frame axes and expressed in meters per second (m/s).
18243 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
18244 * resolved along NED-frame axes and expressed in meters per second (m/s).
18245 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
18246 * resolved along NED-frame axes and expressed in meters per second (m/s).
18247 * @param kinematics body kinematics containing specific forces and angular rates applied to
18248 * the body.
18249 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
18250 * @return estimated NED frame containing new body position, velocity and coordinate
18251 * transformation matrix.
18252 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
18253 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
18254 * body-to-NED-frame coordinate transformation matrix are
18255 * invalid.
18256 */
18257 public static NEDFrame navigateNEDAndReturnNew(
18258 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
18259 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
18260 final BodyKinematics kinematics, final double accuracyThreshold) throws InertialNavigatorException,
18261 InvalidSourceAndDestinationFrameTypeException {
18262 final var result = new NEDFrame();
18263 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, kinematics,
18264 accuracyThreshold, result);
18265 return result;
18266 }
18267
18268 /**
18269 * Runs precision local-navigation-frame inertial navigation equations.
18270 * NOTE: only the attitude update and specific force frame transformation
18271 * phases are precise.
18272 *
18273 * @param timeInterval time interval between epochs.
18274 * @param oldLatitude previous latitude expressed in radians (rad).
18275 * @param oldLongitude previous longitude expressed in radians (rad).
18276 * @param oldHeight previous height expressed in meters (m).
18277 * @param oldC previous body-to-NED coordinate transformation.
18278 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
18279 * resolved along NED-frame axes and expressed in meters per second (m/s).
18280 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
18281 * resolved along NED-frame axes and expressed in meters per second (m/s).
18282 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
18283 * resolved along NED-frame axes and expressed in meters per second (m/s).
18284 * @param kinematics body kinematics containing specific forces and angular rates applied to
18285 * the body.
18286 * @return estimated NED frame containing new body position, velocity and coordinate
18287 * transformation matrix.
18288 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
18289 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
18290 * body-to-NED-frame coordinate transformation matrix are
18291 * invalid.
18292 */
18293 public static NEDFrame navigateNEDAndReturnNew(
18294 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
18295 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
18296 final BodyKinematics kinematics) throws InertialNavigatorException,
18297 InvalidSourceAndDestinationFrameTypeException {
18298 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
18299 kinematics, DEFAULT_ACCURACY_THRESHOLD);
18300 }
18301
18302 /**
18303 * Runs precision local-navigation-frame inertial navigation equations.
18304 * NOTE: only the attitude update and specific force frame transformation
18305 * phases are precise.
18306 *
18307 * @param timeInterval time interval between epochs expressed in seconds (s).
18308 * @param oldPosition previous curvilinear position expressed in terms of latitude,
18309 * longitude and height.
18310 * @param oldC previous body-to-NED coordinate transformation.
18311 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
18312 * resolved along NED-frame axes and expressed in meters per second (m/s).
18313 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
18314 * resolved along NED-frame axes and expressed in meters per second (m/s).
18315 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
18316 * resolved along NED-frame axes and expressed in meters per second (m/s).
18317 * @param kinematics body kinematics containing specific forces and angular rates applied to
18318 * the body.
18319 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
18320 * @return estimated NED frame containing new body position, velocity and coordinate
18321 * transformation matrix.
18322 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
18323 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
18324 * body-to-NED-frame coordinate transformation matrix are
18325 * invalid.
18326 */
18327 public static NEDFrame navigateNEDAndReturnNew(
18328 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
18329 final double oldVn, final double oldVe, final double oldVd, final BodyKinematics kinematics,
18330 final double accuracyThreshold) throws InertialNavigatorException,
18331 InvalidSourceAndDestinationFrameTypeException {
18332 final var result = new NEDFrame();
18333 navigateNED(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, kinematics, accuracyThreshold, result);
18334 return result;
18335 }
18336
18337 /**
18338 * Runs precision local-navigation-frame inertial navigation equations.
18339 * NOTE: only the attitude update and specific force frame transformation
18340 * phases are precise.
18341 *
18342 * @param timeInterval time interval between epochs expressed in seconds (s).
18343 * @param oldPosition previous curvilinear position expressed in terms of latitude,
18344 * longitude and height.
18345 * @param oldC previous body-to-NED coordinate transformation.
18346 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
18347 * resolved along NED-frame axes and expressed in meters per second (m/s).
18348 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
18349 * resolved along NED-frame axes and expressed in meters per second (m/s).
18350 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
18351 * resolved along NED-frame axes and expressed in meters per second (m/s).
18352 * @param kinematics body kinematics containing specific forces and angular rates applied to
18353 * the body.
18354 * @return estimated NED frame containing new body position, velocity and coordinate
18355 * transformation matrix.
18356 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
18357 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
18358 * body-to-NED-frame coordinate transformation matrix are
18359 * invalid.
18360 */
18361 public static NEDFrame navigateNEDAndReturnNew(
18362 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
18363 final double oldVn, final double oldVe, final double oldVd, final BodyKinematics kinematics)
18364 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
18365 return navigateNEDAndReturnNew(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, kinematics,
18366 DEFAULT_ACCURACY_THRESHOLD);
18367 }
18368
18369 /**
18370 * Runs precision local-navigation-frame inertial navigation equations.
18371 * NOTE: only the attitude update and specific force frame transformation
18372 * phases are precise.
18373 *
18374 * @param timeInterval time interval between epochs.
18375 * @param oldPosition previous curvilinear position expressed in terms of latitude,
18376 * longitude and height.
18377 * @param oldC previous body-to-NED coordinate transformation.
18378 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
18379 * resolved along NED-frame axes and expressed in meters per second (m/s).
18380 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
18381 * resolved along NED-frame axes and expressed in meters per second (m/s).
18382 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
18383 * resolved along NED-frame axes and expressed in meters per second (m/s).
18384 * @param kinematics body kinematics containing specific forces and angular rates applied to
18385 * the body.
18386 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
18387 * @return estimated NED frame containing new body position, velocity and coordinate
18388 * transformation matrix.
18389 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
18390 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
18391 * body-to-NED-frame coordinate transformation matrix are
18392 * invalid.
18393 */
18394 public static NEDFrame navigateNEDAndReturnNew(
18395 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
18396 final double oldVn, final double oldVe, final double oldVd, final BodyKinematics kinematics,
18397 final double accuracyThreshold) throws InertialNavigatorException,
18398 InvalidSourceAndDestinationFrameTypeException {
18399 final var result = new NEDFrame();
18400 navigateNED(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, kinematics, accuracyThreshold, result);
18401 return result;
18402 }
18403
18404 /**
18405 * Runs precision local-navigation-frame inertial navigation equations.
18406 * NOTE: only the attitude update and specific force frame transformation
18407 * phases are precise.
18408 *
18409 * @param timeInterval time interval between epochs.
18410 * @param oldPosition previous curvilinear position expressed in terms of latitude,
18411 * longitude and height.
18412 * @param oldC previous body-to-NED coordinate transformation.
18413 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
18414 * resolved along NED-frame axes and expressed in meters per second (m/s).
18415 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
18416 * resolved along NED-frame axes and expressed in meters per second (m/s).
18417 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
18418 * resolved along NED-frame axes and expressed in meters per second (m/s).
18419 * @param kinematics body kinematics containing specific forces and angular rates applied to
18420 * the body.
18421 * @return estimated NED frame containing new body position, velocity and coordinate
18422 * transformation matrix.
18423 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
18424 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
18425 * body-to-NED-frame coordinate transformation matrix are
18426 * invalid.
18427 */
18428 public static NEDFrame navigateNEDAndReturnNew(
18429 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
18430 final double oldVn, final double oldVe, final double oldVd, final BodyKinematics kinematics)
18431 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
18432 return navigateNEDAndReturnNew(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, kinematics,
18433 DEFAULT_ACCURACY_THRESHOLD);
18434 }
18435
18436 /**
18437 * Runs precision local-navigation-frame inertial navigation equations.
18438 * NOTE: only the attitude update and specific force frame transformation
18439 * phases are precise.
18440 *
18441 * @param timeInterval time interval between epochs expressed in seconds (s).
18442 * @param oldLatitude previous latitude expressed in radians (rad).
18443 * @param oldLongitude previous longitude expressed in radians (rad).
18444 * @param oldHeight previous height expressed in meters (m).
18445 * @param oldC previous body-to-NED coordinate transformation.
18446 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
18447 * along north, east and down axes.
18448 * @param kinematics body kinematics containing specific forces and angular rates applied to
18449 * the body.
18450 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
18451 * @return estimated NED frame containing new body position, velocity and coordinate
18452 * transformation matrix.
18453 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
18454 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
18455 * body-to-NED-frame coordinate transformation matrix are
18456 * invalid.
18457 */
18458 public static NEDFrame navigateNEDAndReturnNew(
18459 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
18460 final CoordinateTransformation oldC, final NEDVelocity oldVelocity, final BodyKinematics kinematics,
18461 final double accuracyThreshold) throws InertialNavigatorException,
18462 InvalidSourceAndDestinationFrameTypeException {
18463 final var result = new NEDFrame();
18464 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, kinematics,
18465 accuracyThreshold, result);
18466 return result;
18467 }
18468
18469 /**
18470 * Runs precision local-navigation-frame inertial navigation equations.
18471 * NOTE: only the attitude update and specific force frame transformation
18472 * phases are precise.
18473 *
18474 * @param timeInterval time interval between epochs expressed in seconds (s).
18475 * @param oldLatitude previous latitude expressed in radians (rad).
18476 * @param oldLongitude previous longitude expressed in radians (rad).
18477 * @param oldHeight previous height expressed in meters (m).
18478 * @param oldC previous body-to-NED coordinate transformation.
18479 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
18480 * along north, east and down axes.
18481 * @param kinematics body kinematics containing specific forces and angular rates applied to
18482 * the body.
18483 * @return estimated NED frame containing new body position, velocity and coordinate
18484 * transformation matrix.
18485 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
18486 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
18487 * body-to-NED-frame coordinate transformation matrix are
18488 * invalid.
18489 */
18490 public static NEDFrame navigateNEDAndReturnNew(
18491 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
18492 final CoordinateTransformation oldC, final NEDVelocity oldVelocity, final BodyKinematics kinematics)
18493 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
18494 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity,
18495 kinematics, DEFAULT_ACCURACY_THRESHOLD);
18496 }
18497
18498 /**
18499 * Runs precision local-navigation-frame inertial navigation equations.
18500 * NOTE: only the attitude update and specific force frame transformation
18501 * phases are precise.
18502 *
18503 * @param timeInterval time interval between epochs.
18504 * @param oldLatitude previous latitude expressed in radians (rad).
18505 * @param oldLongitude previous longitude expressed in radians (rad).
18506 * @param oldHeight previous height expressed in meters (m).
18507 * @param oldC previous body-to-NED coordinate transformation.
18508 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
18509 * along north, east and down axes.
18510 * @param kinematics body kinematics containing specific forces and angular rates applied to
18511 * the body.
18512 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
18513 * @return estimated NED frame containing new body position, velocity and coordinate
18514 * transformation matrix.
18515 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
18516 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
18517 * body-to-NED-frame coordinate transformation matrix are
18518 * invalid.
18519 */
18520 public static NEDFrame navigateNEDAndReturnNew(
18521 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
18522 final CoordinateTransformation oldC, final NEDVelocity oldVelocity, final BodyKinematics kinematics,
18523 final double accuracyThreshold) throws InertialNavigatorException,
18524 InvalidSourceAndDestinationFrameTypeException {
18525 final var result = new NEDFrame();
18526 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, kinematics,
18527 accuracyThreshold, result);
18528 return result;
18529 }
18530
18531 /**
18532 * Runs precision local-navigation-frame inertial navigation equations.
18533 * NOTE: only the attitude update and specific force frame transformation
18534 * phases are precise.
18535 *
18536 * @param timeInterval time interval between epochs.
18537 * @param oldLatitude previous latitude expressed in radians (rad).
18538 * @param oldLongitude previous longitude expressed in radians (rad).
18539 * @param oldHeight previous height expressed in meters (m).
18540 * @param oldC previous body-to-NED coordinate transformation.
18541 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
18542 * along north, east and down axes.
18543 * @param kinematics body kinematics containing specific forces and angular rates applied to
18544 * the body.
18545 * @return estimated NED frame containing new body position, velocity and coordinate
18546 * transformation matrix.
18547 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
18548 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
18549 * body-to-NED-frame coordinate transformation matrix are
18550 * invalid.
18551 */
18552 public static NEDFrame navigateNEDAndReturnNew(
18553 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
18554 final CoordinateTransformation oldC, final NEDVelocity oldVelocity, final BodyKinematics kinematics)
18555 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
18556 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity,
18557 kinematics, DEFAULT_ACCURACY_THRESHOLD);
18558 }
18559
18560 /**
18561 * Runs precision local-navigation-frame inertial navigation equations.
18562 * NOTE: only the attitude update and specific force frame transformation
18563 * phases are precise.
18564 *
18565 * @param timeInterval time interval between epochs expressed in seconds (s).
18566 * @param oldPosition previous curvilinear position expressed in terms of latitude,
18567 * longitude and height.
18568 * @param oldC previous body-to-NED coordinate transformation.
18569 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
18570 * along north, east and down axes.
18571 * @param kinematics body kinematics containing specific forces and angular rates applied to
18572 * the body.
18573 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
18574 * @return estimated NED frame containing new body position, velocity and coordinate
18575 * transformation matrix.
18576 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
18577 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
18578 * body-to-NED-frame coordinate transformation matrix are
18579 * invalid.
18580 */
18581 public static NEDFrame navigateNEDAndReturnNew(
18582 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
18583 final NEDVelocity oldVelocity, final BodyKinematics kinematics, final double accuracyThreshold)
18584 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
18585 final var result = new NEDFrame();
18586 navigateNED(timeInterval, oldPosition, oldC, oldVelocity, kinematics, accuracyThreshold, result);
18587 return result;
18588 }
18589
18590 /**
18591 * Runs precision local-navigation-frame inertial navigation equations.
18592 * NOTE: only the attitude update and specific force frame transformation
18593 * phases are precise.
18594 *
18595 * @param timeInterval time interval between epochs expressed in seconds (s).
18596 * @param oldPosition previous curvilinear position expressed in terms of latitude,
18597 * longitude and height.
18598 * @param oldC previous body-to-NED coordinate transformation.
18599 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
18600 * along north, east and down axes.
18601 * @param kinematics body kinematics containing specific forces and angular rates applied to
18602 * the body.
18603 * @return estimated NED frame containing new body position, velocity and coordinate
18604 * transformation matrix.
18605 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
18606 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
18607 * body-to-NED-frame coordinate transformation matrix are
18608 * invalid.
18609 */
18610 public static NEDFrame navigateNEDAndReturnNew(
18611 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
18612 final NEDVelocity oldVelocity, final BodyKinematics kinematics) throws InertialNavigatorException,
18613 InvalidSourceAndDestinationFrameTypeException {
18614 return navigateNEDAndReturnNew(timeInterval, oldPosition, oldC, oldVelocity, kinematics,
18615 DEFAULT_ACCURACY_THRESHOLD);
18616 }
18617
18618 /**
18619 * Runs precision local-navigation-frame inertial navigation equations.
18620 * NOTE: only the attitude update and specific force frame transformation
18621 * phases are precise.
18622 *
18623 * @param timeInterval time interval between epochs expressed in seconds (s).
18624 * @param oldPosition previous curvilinear position expressed in terms of latitude,
18625 * longitude and height.
18626 * @param oldC previous body-to-NED coordinate transformation.
18627 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
18628 * along north, east and down axes.
18629 * @param kinematics body kinematics containing specific forces and angular rates applied to
18630 * the body.
18631 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
18632 * @return estimated NED frame containing new body position, velocity and coordinate
18633 * transformation matrix.
18634 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
18635 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
18636 * body-to-NED-frame coordinate transformation matrix are
18637 * invalid.
18638 */
18639 public static NEDFrame navigateNEDAndReturnNew(
18640 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
18641 final NEDVelocity oldVelocity, final BodyKinematics kinematics, final double accuracyThreshold)
18642 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
18643 final var result = new NEDFrame();
18644 navigateNED(timeInterval, oldPosition, oldC, oldVelocity, kinematics, accuracyThreshold, result);
18645 return result;
18646 }
18647
18648 /**
18649 * Runs precision local-navigation-frame inertial navigation equations.
18650 * NOTE: only the attitude update and specific force frame transformation
18651 * phases are precise.
18652 *
18653 * @param timeInterval time interval between epochs expressed in seconds (s).
18654 * @param oldPosition previous curvilinear position expressed in terms of latitude,
18655 * longitude and height.
18656 * @param oldC previous body-to-NED coordinate transformation.
18657 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
18658 * along north, east and down axes.
18659 * @param kinematics body kinematics containing specific forces and angular rates applied to
18660 * the body.
18661 * @return estimated NED frame containing new body position, velocity and coordinate
18662 * transformation matrix.
18663 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
18664 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
18665 * body-to-NED-frame coordinate transformation matrix are
18666 * invalid.
18667 */
18668 public static NEDFrame navigateNEDAndReturnNew(
18669 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
18670 final NEDVelocity oldVelocity, final BodyKinematics kinematics) throws InertialNavigatorException,
18671 InvalidSourceAndDestinationFrameTypeException {
18672 return navigateNEDAndReturnNew(timeInterval, oldPosition, oldC, oldVelocity, kinematics,
18673 DEFAULT_ACCURACY_THRESHOLD);
18674 }
18675
18676 /**
18677 * Runs precision local-navigation-frame inertial navigation equations.
18678 * NOTE: only the attitude update and specific force frame transformation
18679 * phases are precise.
18680 *
18681 * @param timeInterval time interval between epochs expressed in seconds (s).
18682 * @param oldLatitude previous latitude angle.
18683 * @param oldLongitude previous longitude angle.
18684 * @param oldHeight previous height.
18685 * @param oldC previous body-to-NED coordinate transformation.
18686 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
18687 * resolved along NED-frame axes and expressed in meters per second (m/s).
18688 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
18689 * resolved along NED-frame axes and expressed in meters per second (m/s).
18690 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
18691 * resolved along NED-frame axes and expressed in meters per second (m/s).
18692 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
18693 * resolved along body-frame axes, averaged over time interval and
18694 * expressed in meters per squared second (m/s^2).
18695 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
18696 * resolved along body-frame axes, averaged over time interval and
18697 * expressed in meters per squared second (m/s^2).
18698 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
18699 * resolved along body-frame axes, averaged over time interval and
18700 * expressed in meters per squared second (m/s^2).
18701 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
18702 * resolved along body-frame axes, averaged over time interval and
18703 * expressed in radians per second (rad/s).
18704 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
18705 * resolved along body-frame axes, averaged over time interval and
18706 * expressed in radians per second (rad/s).
18707 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
18708 * resolved along body-frame axes, averaged over time interval and
18709 * expressed in radians per second (rad/s).
18710 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
18711 * @return estimated NED frame containing new body position, velocity and coordinate
18712 * transformation matrix.
18713 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
18714 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
18715 * body-to-NED-frame coordinate transformation matrix are
18716 * invalid.
18717 */
18718 public static NEDFrame navigateNEDAndReturnNew(
18719 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
18720 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
18721 final double fx, final double fy, final double fz,
18722 final double angularRateX, final double angularRateY, final double angularRateZ,
18723 final double accuracyThreshold) throws InertialNavigatorException,
18724 InvalidSourceAndDestinationFrameTypeException {
18725 final var result = new NEDFrame();
18726 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
18727 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
18728 return result;
18729 }
18730
18731 /**
18732 * Runs precision local-navigation-frame inertial navigation equations.
18733 * NOTE: only the attitude update and specific force frame transformation
18734 * phases are precise.
18735 *
18736 * @param timeInterval time interval between epochs.
18737 * @param oldLatitude previous latitude angle.
18738 * @param oldLongitude previous longitude angle.
18739 * @param oldHeight previous height.
18740 * @param oldC previous body-to-NED coordinate transformation.
18741 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
18742 * resolved along NED-frame axes and expressed in meters per second (m/s).
18743 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
18744 * resolved along NED-frame axes and expressed in meters per second (m/s).
18745 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
18746 * resolved along NED-frame axes and expressed in meters per second (m/s).
18747 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
18748 * resolved along body-frame axes, averaged over time interval and
18749 * expressed in meters per squared second (m/s^2).
18750 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
18751 * resolved along body-frame axes, averaged over time interval and
18752 * expressed in meters per squared second (m/s^2).
18753 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
18754 * resolved along body-frame axes, averaged over time interval and
18755 * expressed in meters per squared second (m/s^2).
18756 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
18757 * resolved along body-frame axes, averaged over time interval and
18758 * expressed in radians per second (rad/s).
18759 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
18760 * resolved along body-frame axes, averaged over time interval and
18761 * expressed in radians per second (rad/s).
18762 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
18763 * resolved along body-frame axes, averaged over time interval and
18764 * expressed in radians per second (rad/s).
18765 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
18766 * @return estimated NED frame containing new body position, velocity and coordinate
18767 * transformation matrix.
18768 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
18769 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
18770 * body-to-NED-frame coordinate transformation matrix are
18771 * invalid.
18772 */
18773 public static NEDFrame navigateNEDAndReturnNew(
18774 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
18775 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
18776 final double fx, final double fy, final double fz,
18777 final double angularRateX, final double angularRateY, final double angularRateZ,
18778 final double accuracyThreshold) throws InertialNavigatorException,
18779 InvalidSourceAndDestinationFrameTypeException {
18780 final var result = new NEDFrame();
18781 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
18782 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
18783 return result;
18784 }
18785
18786 /**
18787 * Runs precision local-navigation-frame inertial navigation equations.
18788 * NOTE: only the attitude update and specific force frame transformation
18789 * phases are precise.
18790 *
18791 * @param timeInterval time interval between epochs expressed in seconds (s).
18792 * @param oldLatitude previous latitude angle.
18793 * @param oldLongitude previous longitude angle.
18794 * @param oldHeight previous height.
18795 * @param oldC previous body-to-NED coordinate transformation.
18796 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
18797 * along north, east and down axes.
18798 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
18799 * resolved along body-frame axes, averaged over time interval and
18800 * expressed in meters per squared second (m/s^2).
18801 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
18802 * resolved along body-frame axes, averaged over time interval and
18803 * expressed in meters per squared second (m/s^2).
18804 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
18805 * resolved along body-frame axes, averaged over time interval and
18806 * expressed in meters per squared second (m/s^2).
18807 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
18808 * resolved along body-frame axes, averaged over time interval and
18809 * expressed in radians per second (rad/s).
18810 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
18811 * resolved along body-frame axes, averaged over time interval and
18812 * expressed in radians per second (rad/s).
18813 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
18814 * resolved along body-frame axes, averaged over time interval and
18815 * expressed in radians per second (rad/s).
18816 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
18817 * @return estimated NED frame containing new body position, velocity and coordinate
18818 * transformation matrix.
18819 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
18820 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
18821 * body-to-NED-frame coordinate transformation matrix are
18822 * invalid.
18823 */
18824 public static NEDFrame navigateNEDAndReturnNew(
18825 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
18826 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
18827 final double fx, final double fy, final double fz,
18828 final double angularRateX, final double angularRateY, final double angularRateZ,
18829 final double accuracyThreshold) throws InertialNavigatorException,
18830 InvalidSourceAndDestinationFrameTypeException {
18831 final var result = new NEDFrame();
18832 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
18833 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
18834 return result;
18835 }
18836
18837 /**
18838 * Runs precision local-navigation-frame inertial navigation equations.
18839 * NOTE: only the attitude update and specific force frame transformation
18840 * phases are precise.
18841 *
18842 * @param timeInterval time interval between epochs.
18843 * @param oldLatitude previous latitude angle.
18844 * @param oldLongitude previous longitude angle.
18845 * @param oldHeight previous height.
18846 * @param oldC previous body-to-NED coordinate transformation.
18847 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
18848 * along north, east and down axes.
18849 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
18850 * resolved along body-frame axes, averaged over time interval and
18851 * expressed in meters per squared second (m/s^2).
18852 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
18853 * resolved along body-frame axes, averaged over time interval and
18854 * expressed in meters per squared second (m/s^2).
18855 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
18856 * resolved along body-frame axes, averaged over time interval and
18857 * expressed in meters per squared second (m/s^2).
18858 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
18859 * resolved along body-frame axes, averaged over time interval and
18860 * expressed in radians per second (rad/s).
18861 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
18862 * resolved along body-frame axes, averaged over time interval and
18863 * expressed in radians per second (rad/s).
18864 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
18865 * resolved along body-frame axes, averaged over time interval and
18866 * expressed in radians per second (rad/s).
18867 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
18868 * @return estimated NED frame containing new body position, velocity and coordinate
18869 * transformation matrix.
18870 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
18871 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
18872 * body-to-NED-frame coordinate transformation matrix are
18873 * invalid.
18874 */
18875 public static NEDFrame navigateNEDAndReturnNew(
18876 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
18877 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
18878 final double fx, final double fy, final double fz,
18879 final double angularRateX, final double angularRateY, final double angularRateZ,
18880 final double accuracyThreshold) throws InertialNavigatorException,
18881 InvalidSourceAndDestinationFrameTypeException {
18882 final var result = new NEDFrame();
18883 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
18884 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
18885 return result;
18886 }
18887
18888 /**
18889 * Runs precision local-navigation-frame inertial navigation equations.
18890 * NOTE: only the attitude update and specific force frame transformation
18891 * phases are precise.
18892 *
18893 * @param timeInterval time interval between epochs expressed in seconds (s).
18894 * @param oldLatitude previous latitude angle.
18895 * @param oldLongitude previous longitude angle.
18896 * @param oldHeight previous height.
18897 * @param oldC previous body-to-NED coordinate transformation.
18898 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
18899 * resolved along NED-frame axes and expressed in meters per second (m/s).
18900 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
18901 * resolved along NED-frame axes and expressed in meters per second (m/s).
18902 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
18903 * resolved along NED-frame axes and expressed in meters per second (m/s).
18904 * @param kinematics body kinematics containing specific forces and angular rates applied to
18905 * the body.
18906 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
18907 * @return estimated NED frame containing new body position, velocity and coordinate
18908 * transformation matrix.
18909 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
18910 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
18911 * body-to-NED-frame coordinate transformation matrix are
18912 * invalid.
18913 */
18914 public static NEDFrame navigateNEDAndReturnNew(
18915 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
18916 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
18917 final BodyKinematics kinematics, final double accuracyThreshold) throws InertialNavigatorException,
18918 InvalidSourceAndDestinationFrameTypeException {
18919 final var result = new NEDFrame();
18920 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, kinematics,
18921 accuracyThreshold, result);
18922 return result;
18923 }
18924
18925 /**
18926 * Runs precision local-navigation-frame inertial navigation equations.
18927 * NOTE: only the attitude update and specific force frame transformation
18928 * phases are precise.
18929 *
18930 * @param timeInterval time interval between epochs expressed in seconds (s).
18931 * @param oldLatitude previous latitude angle.
18932 * @param oldLongitude previous longitude angle.
18933 * @param oldHeight previous height.
18934 * @param oldC previous body-to-NED coordinate transformation.
18935 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
18936 * resolved along NED-frame axes and expressed in meters per second (m/s).
18937 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
18938 * resolved along NED-frame axes and expressed in meters per second (m/s).
18939 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
18940 * resolved along NED-frame axes and expressed in meters per second (m/s).
18941 * @param kinematics body kinematics containing specific forces and angular rates applied to
18942 * the body.
18943 * @return estimated NED frame containing new body position, velocity and coordinate
18944 * transformation matrix.
18945 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
18946 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
18947 * body-to-NED-frame coordinate transformation matrix are
18948 * invalid.
18949 */
18950 public static NEDFrame navigateNEDAndReturnNew(
18951 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
18952 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
18953 final BodyKinematics kinematics) throws InertialNavigatorException,
18954 InvalidSourceAndDestinationFrameTypeException {
18955 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
18956 kinematics, DEFAULT_ACCURACY_THRESHOLD);
18957 }
18958
18959 /**
18960 * Runs precision local-navigation-frame inertial navigation equations.
18961 * NOTE: only the attitude update and specific force frame transformation
18962 * phases are precise.
18963 *
18964 * @param timeInterval time interval between epochs.
18965 * @param oldLatitude previous latitude angle.
18966 * @param oldLongitude previous longitude angle.
18967 * @param oldHeight previous height.
18968 * @param oldC previous body-to-NED coordinate transformation.
18969 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
18970 * resolved along NED-frame axes and expressed in meters per second (m/s).
18971 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
18972 * resolved along NED-frame axes and expressed in meters per second (m/s).
18973 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
18974 * resolved along NED-frame axes and expressed in meters per second (m/s).
18975 * @param kinematics body kinematics containing specific forces and angular rates applied to
18976 * the body.
18977 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
18978 * @return estimated NED frame containing new body position, velocity and coordinate
18979 * transformation matrix.
18980 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
18981 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
18982 * body-to-NED-frame coordinate transformation matrix are
18983 * invalid.
18984 */
18985 public static NEDFrame navigateNEDAndReturnNew(
18986 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
18987 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
18988 final BodyKinematics kinematics, final double accuracyThreshold) throws InertialNavigatorException,
18989 InvalidSourceAndDestinationFrameTypeException {
18990 final var result = new NEDFrame();
18991 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, kinematics,
18992 accuracyThreshold, result);
18993 return result;
18994 }
18995
18996 /**
18997 * Runs precision local-navigation-frame inertial navigation equations.
18998 * NOTE: only the attitude update and specific force frame transformation
18999 * phases are precise.
19000 *
19001 * @param timeInterval time interval between epochs.
19002 * @param oldLatitude previous latitude angle.
19003 * @param oldLongitude previous longitude angle.
19004 * @param oldHeight previous height.
19005 * @param oldC previous body-to-NED coordinate transformation.
19006 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
19007 * resolved along NED-frame axes and expressed in meters per second (m/s).
19008 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
19009 * resolved along NED-frame axes and expressed in meters per second (m/s).
19010 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
19011 * resolved along NED-frame axes and expressed in meters per second (m/s).
19012 * @param kinematics body kinematics containing specific forces and angular rates applied to
19013 * the body.
19014 * @return estimated NED frame containing new body position, velocity and coordinate
19015 * transformation matrix.
19016 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
19017 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
19018 * body-to-NED-frame coordinate transformation matrix are
19019 * invalid.
19020 */
19021 public static NEDFrame navigateNEDAndReturnNew(
19022 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
19023 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
19024 final BodyKinematics kinematics) throws InertialNavigatorException,
19025 InvalidSourceAndDestinationFrameTypeException {
19026 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
19027 kinematics, DEFAULT_ACCURACY_THRESHOLD);
19028 }
19029
19030 /**
19031 * Runs precision local-navigation-frame inertial navigation equations.
19032 * NOTE: only the attitude update and specific force frame transformation
19033 * phases are precise.
19034 *
19035 * @param timeInterval time interval between epochs expressed in seconds (s).
19036 * @param oldLatitude previous latitude angle.
19037 * @param oldLongitude previous longitude angle.
19038 * @param oldHeight previous height.
19039 * @param oldC previous body-to-NED coordinate transformation.
19040 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
19041 * along north, east and down axes.
19042 * @param kinematics body kinematics containing specific forces and angular rates applied to
19043 * the body.
19044 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
19045 * @return estimated NED frame containing new body position, velocity and coordinate
19046 * transformation matrix.
19047 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
19048 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
19049 * body-to-NED-frame coordinate transformation matrix are
19050 * invalid.
19051 */
19052 public static NEDFrame navigateNEDAndReturnNew(
19053 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
19054 final CoordinateTransformation oldC, final NEDVelocity oldVelocity, final BodyKinematics kinematics,
19055 final double accuracyThreshold) throws InertialNavigatorException,
19056 InvalidSourceAndDestinationFrameTypeException {
19057 final var result = new NEDFrame();
19058 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, kinematics,
19059 accuracyThreshold, result);
19060 return result;
19061 }
19062
19063 /**
19064 * Runs precision local-navigation-frame inertial navigation equations.
19065 * NOTE: only the attitude update and specific force frame transformation
19066 * phases are precise.
19067 *
19068 * @param timeInterval time interval between epochs expressed in seconds (s).
19069 * @param oldLatitude previous latitude angle.
19070 * @param oldLongitude previous longitude angle.
19071 * @param oldHeight previous height.
19072 * @param oldC previous body-to-NED coordinate transformation.
19073 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
19074 * along north, east and down axes.
19075 * @param kinematics body kinematics containing specific forces and angular rates applied to
19076 * the body.
19077 * @return estimated NED frame containing new body position, velocity and coordinate
19078 * transformation matrix.
19079 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
19080 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
19081 * body-to-NED-frame coordinate transformation matrix are
19082 * invalid.
19083 */
19084 public static NEDFrame navigateNEDAndReturnNew(
19085 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
19086 final CoordinateTransformation oldC, final NEDVelocity oldVelocity, final BodyKinematics kinematics)
19087 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
19088 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity,
19089 kinematics, DEFAULT_ACCURACY_THRESHOLD);
19090 }
19091
19092 /**
19093 * Runs precision local-navigation-frame inertial navigation equations.
19094 * NOTE: only the attitude update and specific force frame transformation
19095 * phases are precise.
19096 *
19097 * @param timeInterval time interval between epochs.
19098 * @param oldLatitude previous latitude angle.
19099 * @param oldLongitude previous longitude angle.
19100 * @param oldHeight previous height.
19101 * @param oldC previous body-to-NED coordinate transformation.
19102 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
19103 * along north, east and down axes.
19104 * @param kinematics body kinematics containing specific forces and angular rates applied to
19105 * the body.
19106 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
19107 * @return estimated NED frame containing new body position, velocity and coordinate
19108 * transformation matrix.
19109 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
19110 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
19111 * body-to-NED-frame coordinate transformation matrix are
19112 * invalid.
19113 */
19114 public static NEDFrame navigateNEDAndReturnNew(
19115 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
19116 final CoordinateTransformation oldC, final NEDVelocity oldVelocity, final BodyKinematics kinematics,
19117 final double accuracyThreshold) throws InertialNavigatorException,
19118 InvalidSourceAndDestinationFrameTypeException {
19119 final var result = new NEDFrame();
19120 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, kinematics,
19121 accuracyThreshold, result);
19122 return result;
19123 }
19124
19125 /**
19126 * Runs precision local-navigation-frame inertial navigation equations.
19127 * NOTE: only the attitude update and specific force frame transformation
19128 * phases are precise.
19129 *
19130 * @param timeInterval time interval between epochs.
19131 * @param oldLatitude previous latitude angle.
19132 * @param oldLongitude previous longitude angle.
19133 * @param oldHeight previous height.
19134 * @param oldC previous body-to-NED coordinate transformation.
19135 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
19136 * along north, east and down axes.
19137 * @param kinematics body kinematics containing specific forces and angular rates applied to
19138 * the body.
19139 * @return estimated NED frame containing new body position, velocity and coordinate
19140 * transformation matrix.
19141 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
19142 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
19143 * body-to-NED-frame coordinate transformation matrix are
19144 * invalid.
19145 */
19146 public static NEDFrame navigateNEDAndReturnNew(
19147 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
19148 final CoordinateTransformation oldC, final NEDVelocity oldVelocity, final BodyKinematics kinematics)
19149 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
19150 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity,
19151 kinematics, DEFAULT_ACCURACY_THRESHOLD);
19152 }
19153
19154 /**
19155 * Runs precision local-navigation-frame inertial navigation equations.
19156 * NOTE: only the attitude update and specific force frame transformation
19157 * phases are precise.
19158 *
19159 * @param timeInterval time interval between epochs expressed in seconds (s).
19160 * @param oldLatitude previous latitude expressed in radians (rad).
19161 * @param oldLongitude previous longitude expressed in radians (rad).
19162 * @param oldHeight previous height expressed in meters (m).
19163 * @param oldC previous body-to-NED coordinate transformation.
19164 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
19165 * resolved along NED-frame axes.
19166 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
19167 * resolved along NED-frame axes.
19168 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
19169 * resolved along NED-frame axes.
19170 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
19171 * resolved along body-frame axes, averaged over time interval and
19172 * expressed in meters per squared second (m/s^2).
19173 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
19174 * resolved along body-frame axes, averaged over time interval and
19175 * expressed in meters per squared second (m/s^2).
19176 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
19177 * resolved along body-frame axes, averaged over time interval and
19178 * expressed in meters per squared second (m/s^2).
19179 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
19180 * resolved along body-frame axes, averaged over time interval and
19181 * expressed in radians per second (rad/s).
19182 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
19183 * resolved along body-frame axes, averaged over time interval and
19184 * expressed in radians per second (rad/s).
19185 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
19186 * resolved along body-frame axes, averaged over time interval and
19187 * expressed in radians per second (rad/s).
19188 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
19189 * @return estimated NED frame containing new body position, velocity and coordinate
19190 * transformation matrix.
19191 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
19192 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
19193 * body-to-NED-frame coordinate transformation matrix are
19194 * invalid.
19195 */
19196 public static NEDFrame navigateNEDAndReturnNew(
19197 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
19198 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
19199 final double fx, final double fy, final double fz,
19200 final double angularRateX, final double angularRateY, final double angularRateZ,
19201 final double accuracyThreshold) throws InertialNavigatorException,
19202 InvalidSourceAndDestinationFrameTypeException {
19203 final var result = new NEDFrame();
19204 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
19205 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
19206 return result;
19207 }
19208
19209 /**
19210 * Runs precision local-navigation-frame inertial navigation equations.
19211 * NOTE: only the attitude update and specific force frame transformation
19212 * phases are precise.
19213 *
19214 * @param timeInterval time interval between epochs.
19215 * @param oldLatitude previous latitude expressed in radians (rad).
19216 * @param oldLongitude previous longitude expressed in radians (rad).
19217 * @param oldHeight previous height expressed in meters (m).
19218 * @param oldC previous body-to-NED coordinate transformation.
19219 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
19220 * resolved along NED-frame axes.
19221 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
19222 * resolved along NED-frame axes.
19223 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
19224 * resolved along NED-frame axes.
19225 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
19226 * resolved along body-frame axes, averaged over time interval and
19227 * expressed in meters per squared second (m/s^2).
19228 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
19229 * resolved along body-frame axes, averaged over time interval and
19230 * expressed in meters per squared second (m/s^2).
19231 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
19232 * resolved along body-frame axes, averaged over time interval and
19233 * expressed in meters per squared second (m/s^2).
19234 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
19235 * resolved along body-frame axes, averaged over time interval and
19236 * expressed in radians per second (rad/s).
19237 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
19238 * resolved along body-frame axes, averaged over time interval and
19239 * expressed in radians per second (rad/s).
19240 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
19241 * resolved along body-frame axes, averaged over time interval and
19242 * expressed in radians per second (rad/s).
19243 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
19244 * @return estimated NED frame containing new body position, velocity and coordinate
19245 * transformation matrix.
19246 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
19247 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
19248 * body-to-NED-frame coordinate transformation matrix are
19249 * invalid.
19250 */
19251 public static NEDFrame navigateNEDAndReturnNew(
19252 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
19253 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
19254 final double fx, final double fy, final double fz,
19255 final double angularRateX, final double angularRateY, final double angularRateZ,
19256 final double accuracyThreshold) throws InertialNavigatorException,
19257 InvalidSourceAndDestinationFrameTypeException {
19258 final var result = new NEDFrame();
19259 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
19260 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
19261 return result;
19262 }
19263
19264 /**
19265 * Runs precision local-navigation-frame inertial navigation equations.
19266 * NOTE: only the attitude update and specific force frame transformation
19267 * phases are precise.
19268 *
19269 * @param timeInterval time interval between epochs expressed in seconds (s).
19270 * @param oldPosition previous curvilinear position expressed in terms of latitude,
19271 * longitude and height.
19272 * @param oldC previous body-to-NED coordinate transformation.
19273 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
19274 * resolved along NED-frame axes.
19275 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
19276 * resolved along NED-frame axes.
19277 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
19278 * resolved along NED-frame axes.
19279 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
19280 * resolved along body-frame axes, averaged over time interval and
19281 * expressed in meters per squared second (m/s^2).
19282 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
19283 * resolved along body-frame axes, averaged over time interval and
19284 * expressed in meters per squared second (m/s^2).
19285 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
19286 * resolved along body-frame axes, averaged over time interval and
19287 * expressed in meters per squared second (m/s^2).
19288 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
19289 * resolved along body-frame axes, averaged over time interval and
19290 * expressed in radians per second (rad/s).
19291 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
19292 * resolved along body-frame axes, averaged over time interval and
19293 * expressed in radians per second (rad/s).
19294 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
19295 * resolved along body-frame axes, averaged over time interval and
19296 * expressed in radians per second (rad/s).
19297 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
19298 * @return estimated NED frame containing new body position, velocity and coordinate
19299 * transformation matrix.
19300 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
19301 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
19302 * body-to-NED-frame coordinate transformation matrix are
19303 * invalid.
19304 */
19305 public static NEDFrame navigateNEDAndReturnNew(
19306 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
19307 final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
19308 final double fx, final double fy, final double fz,
19309 final double angularRateX, final double angularRateY, final double angularRateZ,
19310 final double accuracyThreshold) throws InertialNavigatorException,
19311 InvalidSourceAndDestinationFrameTypeException {
19312 final var result = new NEDFrame();
19313 navigateNED(timeInterval, oldPosition, oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
19314 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
19315 return result;
19316 }
19317
19318 /**
19319 * Runs precision local-navigation-frame inertial navigation equations.
19320 * NOTE: only the attitude update and specific force frame transformation
19321 * phases are precise.
19322 *
19323 * @param timeInterval time interval between epochs.
19324 * @param oldPosition previous curvilinear position expressed in terms of latitude,
19325 * longitude and height.
19326 * @param oldC previous body-to-NED coordinate transformation.
19327 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
19328 * resolved along NED-frame axes.
19329 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
19330 * resolved along NED-frame axes.
19331 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
19332 * resolved along NED-frame axes.
19333 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
19334 * resolved along body-frame axes, averaged over time interval and
19335 * expressed in meters per squared second (m/s^2).
19336 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
19337 * resolved along body-frame axes, averaged over time interval and
19338 * expressed in meters per squared second (m/s^2).
19339 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
19340 * resolved along body-frame axes, averaged over time interval and
19341 * expressed in meters per squared second (m/s^2).
19342 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
19343 * resolved along body-frame axes, averaged over time interval and
19344 * expressed in radians per second (rad/s).
19345 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
19346 * resolved along body-frame axes, averaged over time interval and
19347 * expressed in radians per second (rad/s).
19348 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
19349 * resolved along body-frame axes, averaged over time interval and
19350 * expressed in radians per second (rad/s).
19351 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
19352 * @return estimated NED frame containing new body position, velocity and coordinate
19353 * transformation matrix.
19354 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
19355 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
19356 * body-to-NED-frame coordinate transformation matrix are
19357 * invalid.
19358 */
19359 public static NEDFrame navigateNEDAndReturnNew(
19360 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
19361 final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
19362 final double fx, final double fy, final double fz,
19363 final double angularRateX, final double angularRateY, final double angularRateZ,
19364 final double accuracyThreshold) throws InertialNavigatorException,
19365 InvalidSourceAndDestinationFrameTypeException {
19366 final var result = new NEDFrame();
19367 navigateNED(timeInterval, oldPosition, oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
19368 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
19369 return result;
19370 }
19371
19372 /**
19373 * Runs precision local-navigation-frame inertial navigation equations.
19374 * NOTE: only the attitude update and specific force frame transformation
19375 * phases are precise.
19376 *
19377 * @param timeInterval time interval between epochs expressed in seconds (s).
19378 * @param oldLatitude previous latitude expressed in radians (rad).
19379 * @param oldLongitude previous longitude expressed in radians (rad).
19380 * @param oldHeight previous height expressed in meters (m).
19381 * @param oldC previous body-to-NED coordinate transformation.
19382 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
19383 * resolved along NED-frame axes.
19384 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
19385 * resolved along NED-frame axes.
19386 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
19387 * resolved along NED-frame axes.
19388 * @param kinematics body kinematics containing specific forces and angular rates applied to
19389 * the body.
19390 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
19391 * @return estimated NED frame containing new body position, velocity and coordinate
19392 * transformation matrix.
19393 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
19394 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
19395 * body-to-NED-frame coordinate transformation matrix are
19396 * invalid.
19397 */
19398 public static NEDFrame navigateNEDAndReturnNew(
19399 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
19400 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
19401 final BodyKinematics kinematics, final double accuracyThreshold) throws InertialNavigatorException,
19402 InvalidSourceAndDestinationFrameTypeException {
19403 final var result = new NEDFrame();
19404 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
19405 kinematics, accuracyThreshold, result);
19406 return result;
19407 }
19408
19409 /**
19410 * Runs precision local-navigation-frame inertial navigation equations.
19411 * NOTE: only the attitude update and specific force frame transformation
19412 * phases are precise.
19413 *
19414 * @param timeInterval time interval between epochs expressed in seconds (s).
19415 * @param oldLatitude previous latitude expressed in radians (rad).
19416 * @param oldLongitude previous longitude expressed in radians (rad).
19417 * @param oldHeight previous height expressed in meters (m).
19418 * @param oldC previous body-to-NED coordinate transformation.
19419 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
19420 * resolved along NED-frame axes.
19421 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
19422 * resolved along NED-frame axes.
19423 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
19424 * resolved along NED-frame axes.
19425 * @param kinematics body kinematics containing specific forces and angular rates applied to
19426 * the body.
19427 * @return estimated NED frame containing new body position, velocity and coordinate
19428 * transformation matrix.
19429 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
19430 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
19431 * body-to-NED-frame coordinate transformation matrix are
19432 * invalid.
19433 */
19434 public static NEDFrame navigateNEDAndReturnNew(
19435 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
19436 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
19437 final BodyKinematics kinematics) throws InertialNavigatorException,
19438 InvalidSourceAndDestinationFrameTypeException {
19439 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
19440 oldSpeedN, oldSpeedE, oldSpeedD, kinematics, DEFAULT_ACCURACY_THRESHOLD);
19441 }
19442
19443 /**
19444 * Runs precision local-navigation-frame inertial navigation equations.
19445 * NOTE: only the attitude update and specific force frame transformation
19446 * phases are precise.
19447 *
19448 * @param timeInterval time interval between epochs.
19449 * @param oldLatitude previous latitude expressed in radians (rad).
19450 * @param oldLongitude previous longitude expressed in radians (rad).
19451 * @param oldHeight previous height expressed in meters (m).
19452 * @param oldC previous body-to-NED coordinate transformation.
19453 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
19454 * resolved along NED-frame axes.
19455 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
19456 * resolved along NED-frame axes.
19457 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
19458 * resolved along NED-frame axes.
19459 * @param kinematics body kinematics containing specific forces and angular rates applied to
19460 * the body.
19461 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
19462 * @return estimated NED frame containing new body position, velocity and coordinate
19463 * transformation matrix.
19464 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
19465 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
19466 * body-to-NED-frame coordinate transformation matrix are
19467 * invalid.
19468 */
19469 public static NEDFrame navigateNEDAndReturnNew(
19470 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
19471 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
19472 final BodyKinematics kinematics, final double accuracyThreshold) throws InertialNavigatorException,
19473 InvalidSourceAndDestinationFrameTypeException {
19474 final var result = new NEDFrame();
19475 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
19476 kinematics, accuracyThreshold, result);
19477 return result;
19478 }
19479
19480 /**
19481 * Runs precision local-navigation-frame inertial navigation equations.
19482 * NOTE: only the attitude update and specific force frame transformation
19483 * phases are precise.
19484 *
19485 * @param timeInterval time interval between epochs.
19486 * @param oldLatitude previous latitude expressed in radians (rad).
19487 * @param oldLongitude previous longitude expressed in radians (rad).
19488 * @param oldHeight previous height expressed in meters (m).
19489 * @param oldC previous body-to-NED coordinate transformation.
19490 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
19491 * resolved along NED-frame axes.
19492 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
19493 * resolved along NED-frame axes.
19494 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
19495 * resolved along NED-frame axes.
19496 * @param kinematics body kinematics containing specific forces and angular rates applied to
19497 * the body.
19498 * @return estimated NED frame containing new body position, velocity and coordinate
19499 * transformation matrix.
19500 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
19501 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
19502 * body-to-NED-frame coordinate transformation matrix are
19503 * invalid.
19504 */
19505 public static NEDFrame navigateNEDAndReturnNew(
19506 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
19507 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
19508 final BodyKinematics kinematics) throws InertialNavigatorException,
19509 InvalidSourceAndDestinationFrameTypeException {
19510 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
19511 oldSpeedN, oldSpeedE, oldSpeedD, kinematics, DEFAULT_ACCURACY_THRESHOLD);
19512 }
19513
19514 /**
19515 * Runs precision local-navigation-frame inertial navigation equations.
19516 * NOTE: only the attitude update and specific force frame transformation
19517 * phases are precise.
19518 *
19519 * @param timeInterval time interval between epochs expressed in seconds (s).
19520 * @param oldPosition previous curvilinear position expressed in terms of latitude,
19521 * longitude and height.
19522 * @param oldC previous body-to-NED coordinate transformation.
19523 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
19524 * resolved along NED-frame axes.
19525 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
19526 * resolved along NED-frame axes.
19527 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
19528 * resolved along NED-frame axes.
19529 * @param kinematics body kinematics containing specific forces and angular rates applied to
19530 * the body.
19531 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
19532 * @return estimated NED frame containing new body position, velocity and coordinate
19533 * transformation matrix.
19534 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
19535 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
19536 * body-to-NED-frame coordinate transformation matrix are
19537 * invalid.
19538 */
19539 public static NEDFrame navigateNEDAndReturnNew(
19540 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
19541 final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD, final BodyKinematics kinematics,
19542 final double accuracyThreshold) throws InertialNavigatorException,
19543 InvalidSourceAndDestinationFrameTypeException {
19544 final var result = new NEDFrame();
19545 navigateNED(timeInterval, oldPosition, oldC, oldSpeedN, oldSpeedE, oldSpeedD, kinematics, accuracyThreshold,
19546 result);
19547 return result;
19548 }
19549
19550 /**
19551 * Runs precision local-navigation-frame inertial navigation equations.
19552 * NOTE: only the attitude update and specific force frame transformation
19553 * phases are precise.
19554 *
19555 * @param timeInterval time interval between epochs expressed in seconds (s).
19556 * @param oldPosition previous curvilinear position expressed in terms of latitude,
19557 * longitude and height.
19558 * @param oldC previous body-to-NED coordinate transformation.
19559 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
19560 * resolved along NED-frame axes.
19561 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
19562 * resolved along NED-frame axes.
19563 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
19564 * resolved along NED-frame axes.
19565 * @param kinematics body kinematics containing specific forces and angular rates applied to
19566 * the body.
19567 * @return estimated NED frame containing new body position, velocity and coordinate
19568 * transformation matrix.
19569 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
19570 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
19571 * body-to-NED-frame coordinate transformation matrix are
19572 * invalid.
19573 */
19574 public static NEDFrame navigateNEDAndReturnNew(
19575 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
19576 final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD, final BodyKinematics kinematics)
19577 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
19578 return navigateNEDAndReturnNew(timeInterval, oldPosition, oldC, oldSpeedN, oldSpeedE, oldSpeedD, kinematics,
19579 DEFAULT_ACCURACY_THRESHOLD);
19580 }
19581
19582 /**
19583 * Runs precision local-navigation-frame inertial navigation equations.
19584 * NOTE: only the attitude update and specific force frame transformation
19585 * phases are precise.
19586 *
19587 * @param timeInterval time interval between epochs.
19588 * @param oldPosition previous curvilinear position expressed in terms of latitude,
19589 * longitude and height.
19590 * @param oldC previous body-to-NED coordinate transformation.
19591 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
19592 * resolved along NED-frame axes.
19593 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
19594 * resolved along NED-frame axes.
19595 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
19596 * resolved along NED-frame axes.
19597 * @param kinematics body kinematics containing specific forces and angular rates applied to
19598 * the body.
19599 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
19600 * @return estimated NED frame containing new body position, velocity and coordinate
19601 * transformation matrix.
19602 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
19603 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
19604 * body-to-NED-frame coordinate transformation matrix are
19605 * invalid.
19606 */
19607 public static NEDFrame navigateNEDAndReturnNew(
19608 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
19609 final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD, final BodyKinematics kinematics,
19610 final double accuracyThreshold) throws InertialNavigatorException,
19611 InvalidSourceAndDestinationFrameTypeException {
19612 final var result = new NEDFrame();
19613 navigateNED(timeInterval, oldPosition, oldC, oldSpeedN, oldSpeedE, oldSpeedD, kinematics, accuracyThreshold,
19614 result);
19615 return result;
19616 }
19617
19618 /**
19619 * Runs precision local-navigation-frame inertial navigation equations.
19620 * NOTE: only the attitude update and specific force frame transformation
19621 * phases are precise.
19622 *
19623 * @param timeInterval time interval between epochs.
19624 * @param oldPosition previous curvilinear position expressed in terms of latitude,
19625 * longitude and height.
19626 * @param oldC previous body-to-NED coordinate transformation.
19627 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
19628 * resolved along NED-frame axes.
19629 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
19630 * resolved along NED-frame axes.
19631 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
19632 * resolved along NED-frame axes.
19633 * @param kinematics body kinematics containing specific forces and angular rates applied to
19634 * the body.
19635 * @return estimated NED frame containing new body position, velocity and coordinate
19636 * transformation matrix.
19637 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
19638 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
19639 * body-to-NED-frame coordinate transformation matrix are
19640 * invalid.
19641 */
19642 public static NEDFrame navigateNEDAndReturnNew(
19643 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
19644 final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD, final BodyKinematics kinematics)
19645 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
19646 return navigateNEDAndReturnNew(timeInterval, oldPosition, oldC, oldSpeedN, oldSpeedE, oldSpeedD, kinematics,
19647 DEFAULT_ACCURACY_THRESHOLD);
19648 }
19649
19650 /**
19651 * Runs precision local-navigation-frame inertial navigation equations.
19652 * NOTE: only the attitude update and specific force frame transformation
19653 * phases are precise.
19654 *
19655 * @param timeInterval time interval between epochs expressed in seconds (s).
19656 * @param oldLatitude previous latitude expressed in radians (rad).
19657 * @param oldLongitude previous longitude expressed in radians (rad).
19658 * @param oldHeight previous height expressed in meters (m).
19659 * @param oldC previous body-to-NED coordinate transformation.
19660 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
19661 * resolved along NED-frame axes and expressed in meters per second (m/s).
19662 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
19663 * resolved along NED-frame axes and expressed in meters per second (m/s).
19664 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
19665 * resolved along NED-frame axes and expressed in meters per second (m/s).
19666 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
19667 * resolved along body-frame axes, averaged over time interval.
19668 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
19669 * resolved along body-frame axes, averaged over time interval.
19670 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
19671 * resolved along body-frame axes, averaged over time interval.
19672 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
19673 * resolved along body-frame axes, averaged over time interval and
19674 * expressed in radians per second (rad/s).
19675 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
19676 * resolved along body-frame axes, averaged over time interval and
19677 * expressed in radians per second (rad/s).
19678 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
19679 * resolved along body-frame axes, averaged over time interval and
19680 * expressed in radians per second (rad/s).
19681 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
19682 * @return estimated NED frame containing new body position, velocity and coordinate
19683 * transformation matrix.
19684 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
19685 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
19686 * body-to-NED-frame coordinate transformation matrix are
19687 * invalid.
19688 */
19689 public static NEDFrame navigateNEDAndReturnNew(
19690 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
19691 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
19692 final Acceleration fx, final Acceleration fy, final Acceleration fz,
19693 final double angularRateX, final double angularRateY, final double angularRateZ,
19694 final double accuracyThreshold) throws InertialNavigatorException,
19695 InvalidSourceAndDestinationFrameTypeException {
19696 final var result = new NEDFrame();
19697 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
19698 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
19699 return result;
19700 }
19701
19702 /**
19703 * Runs precision local-navigation-frame inertial navigation equations.
19704 * NOTE: only the attitude update and specific force frame transformation
19705 * phases are precise.
19706 *
19707 * @param timeInterval time interval between epochs.
19708 * @param oldLatitude previous latitude expressed in radians (rad).
19709 * @param oldLongitude previous longitude expressed in radians (rad).
19710 * @param oldHeight previous height expressed in meters (m).
19711 * @param oldC previous body-to-NED coordinate transformation.
19712 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
19713 * resolved along NED-frame axes and expressed in meters per second (m/s).
19714 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
19715 * resolved along NED-frame axes and expressed in meters per second (m/s).
19716 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
19717 * resolved along NED-frame axes and expressed in meters per second (m/s).
19718 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
19719 * resolved along body-frame axes, averaged over time interval.
19720 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
19721 * resolved along body-frame axes, averaged over time interval.
19722 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
19723 * resolved along body-frame axes, averaged over time interval.
19724 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
19725 * resolved along body-frame axes, averaged over time interval and
19726 * expressed in radians per second (rad/s).
19727 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
19728 * resolved along body-frame axes, averaged over time interval and
19729 * expressed in radians per second (rad/s).
19730 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
19731 * resolved along body-frame axes, averaged over time interval and
19732 * expressed in radians per second (rad/s).
19733 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
19734 * @return estimated NED frame containing new body position, velocity and coordinate
19735 * transformation matrix.
19736 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
19737 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
19738 * body-to-NED-frame coordinate transformation matrix are
19739 * invalid.
19740 */
19741 public static NEDFrame navigateNEDAndReturnNew(
19742 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
19743 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
19744 final Acceleration fx, final Acceleration fy, final Acceleration fz,
19745 final double angularRateX, final double angularRateY, final double angularRateZ,
19746 final double accuracyThreshold) throws InertialNavigatorException,
19747 InvalidSourceAndDestinationFrameTypeException {
19748 final var result = new NEDFrame();
19749 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
19750 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
19751 return result;
19752 }
19753
19754 /**
19755 * Runs precision local-navigation-frame inertial navigation equations.
19756 * NOTE: only the attitude update and specific force frame transformation
19757 * phases are precise.
19758 *
19759 * @param timeInterval time interval between epochs expressed in seconds (s).
19760 * @param oldPosition previous curvilinear position expressed in terms of latitude,
19761 * longitude and height.
19762 * @param oldC previous body-to-NED coordinate transformation.
19763 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
19764 * resolved along NED-frame axes and expressed in meters per second (m/s).
19765 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
19766 * resolved along NED-frame axes and expressed in meters per second (m/s).
19767 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
19768 * resolved along NED-frame axes and expressed in meters per second (m/s).
19769 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
19770 * resolved along body-frame axes, averaged over time interval.
19771 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
19772 * resolved along body-frame axes, averaged over time interval.
19773 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
19774 * resolved along body-frame axes, averaged over time interval.
19775 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
19776 * resolved along body-frame axes, averaged over time interval and
19777 * expressed in radians per second (rad/s).
19778 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
19779 * resolved along body-frame axes, averaged over time interval and
19780 * expressed in radians per second (rad/s).
19781 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
19782 * resolved along body-frame axes, averaged over time interval and
19783 * expressed in radians per second (rad/s).
19784 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
19785 * @return estimated NED frame containing new body position, velocity and coordinate
19786 * transformation matrix.
19787 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
19788 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
19789 * body-to-NED-frame coordinate transformation matrix are
19790 * invalid.
19791 */
19792 public static NEDFrame navigateNEDAndReturnNew(
19793 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
19794 final double oldVn, final double oldVe, final double oldVd,
19795 final Acceleration fx, final Acceleration fy, final Acceleration fz,
19796 final double angularRateX, final double angularRateY, final double angularRateZ,
19797 final double accuracyThreshold) throws InertialNavigatorException,
19798 InvalidSourceAndDestinationFrameTypeException {
19799 final var result = new NEDFrame();
19800 navigateNED(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
19801 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
19802 return result;
19803 }
19804
19805 /**
19806 * Runs precision local-navigation-frame inertial navigation equations.
19807 * NOTE: only the attitude update and specific force frame transformation
19808 * phases are precise.
19809 *
19810 * @param timeInterval time interval between epochs.
19811 * @param oldPosition previous curvilinear position expressed in terms of latitude,
19812 * longitude and height.
19813 * @param oldC previous body-to-NED coordinate transformation.
19814 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
19815 * resolved along NED-frame axes and expressed in meters per second (m/s).
19816 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
19817 * resolved along NED-frame axes and expressed in meters per second (m/s).
19818 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
19819 * resolved along NED-frame axes and expressed in meters per second (m/s).
19820 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
19821 * resolved along body-frame axes, averaged over time interval.
19822 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
19823 * resolved along body-frame axes, averaged over time interval.
19824 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
19825 * resolved along body-frame axes, averaged over time interval.
19826 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
19827 * resolved along body-frame axes, averaged over time interval and
19828 * expressed in radians per second (rad/s).
19829 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
19830 * resolved along body-frame axes, averaged over time interval and
19831 * expressed in radians per second (rad/s).
19832 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
19833 * resolved along body-frame axes, averaged over time interval and
19834 * expressed in radians per second (rad/s).
19835 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
19836 * @return estimated NED frame containing new body position, velocity and coordinate
19837 * transformation matrix.
19838 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
19839 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
19840 * body-to-NED-frame coordinate transformation matrix are
19841 * invalid.
19842 */
19843 public static NEDFrame navigateNEDAndReturnNew(
19844 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
19845 final double oldVn, final double oldVe, final double oldVd,
19846 final Acceleration fx, final Acceleration fy, final Acceleration fz,
19847 final double angularRateX, final double angularRateY, final double angularRateZ,
19848 final double accuracyThreshold) throws InertialNavigatorException,
19849 InvalidSourceAndDestinationFrameTypeException {
19850 final var result = new NEDFrame();
19851 navigateNED(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
19852 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
19853 return result;
19854 }
19855
19856 /**
19857 * Runs precision local-navigation-frame inertial navigation equations.
19858 * NOTE: only the attitude update and specific force frame transformation
19859 * phases are precise.
19860 *
19861 * @param timeInterval time interval between epochs expressed in seconds (s).
19862 * @param oldLatitude previous latitude expressed in radians (rad).
19863 * @param oldLongitude previous longitude expressed in radians (rad).
19864 * @param oldHeight previous height expressed in meters (m).
19865 * @param oldC previous body-to-NED coordinate transformation.
19866 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
19867 * along north, east and down axes.
19868 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
19869 * resolved along body-frame axes, averaged over time interval.
19870 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
19871 * resolved along body-frame axes, averaged over time interval.
19872 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
19873 * resolved along body-frame axes, averaged over time interval.
19874 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
19875 * resolved along body-frame axes, averaged over time interval and
19876 * expressed in radians per second (rad/s).
19877 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
19878 * resolved along body-frame axes, averaged over time interval and
19879 * expressed in radians per second (rad/s).
19880 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
19881 * resolved along body-frame axes, averaged over time interval and
19882 * expressed in radians per second (rad/s).
19883 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
19884 * @return estimated NED frame containing new body position, velocity and coordinate
19885 * transformation matrix.
19886 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
19887 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
19888 * body-to-NED-frame coordinate transformation matrix are
19889 * invalid.
19890 */
19891 public static NEDFrame navigateNEDAndReturnNew(
19892 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
19893 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
19894 final Acceleration fx, final Acceleration fy, final Acceleration fz,
19895 final double angularRateX, final double angularRateY, final double angularRateZ,
19896 final double accuracyThreshold) throws InertialNavigatorException,
19897 InvalidSourceAndDestinationFrameTypeException {
19898 final var result = new NEDFrame();
19899 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
19900 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
19901 return result;
19902 }
19903
19904 /**
19905 * Runs precision local-navigation-frame inertial navigation equations.
19906 * NOTE: only the attitude update and specific force frame transformation
19907 * phases are precise.
19908 *
19909 * @param timeInterval time interval between epochs.
19910 * @param oldLatitude previous latitude expressed in radians (rad).
19911 * @param oldLongitude previous longitude expressed in radians (rad).
19912 * @param oldHeight previous height expressed in meters (m).
19913 * @param oldC previous body-to-NED coordinate transformation.
19914 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
19915 * along north, east and down axes.
19916 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
19917 * resolved along body-frame axes, averaged over time interval.
19918 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
19919 * resolved along body-frame axes, averaged over time interval.
19920 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
19921 * resolved along body-frame axes, averaged over time interval.
19922 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
19923 * resolved along body-frame axes, averaged over time interval and
19924 * expressed in radians per second (rad/s).
19925 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
19926 * resolved along body-frame axes, averaged over time interval and
19927 * expressed in radians per second (rad/s).
19928 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
19929 * resolved along body-frame axes, averaged over time interval and
19930 * expressed in radians per second (rad/s).
19931 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
19932 * @return estimated NED frame containing new body position, velocity and coordinate
19933 * transformation matrix.
19934 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
19935 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
19936 * body-to-NED-frame coordinate transformation matrix are
19937 * invalid.
19938 */
19939 public static NEDFrame navigateNEDAndReturnNew(
19940 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
19941 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
19942 final Acceleration fx, final Acceleration fy, final Acceleration fz,
19943 final double angularRateX, final double angularRateY, final double angularRateZ,
19944 final double accuracyThreshold) throws InertialNavigatorException,
19945 InvalidSourceAndDestinationFrameTypeException {
19946 final var result = new NEDFrame();
19947 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
19948 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
19949 return result;
19950 }
19951
19952 /**
19953 * Runs precision local-navigation-frame inertial navigation equations.
19954 * NOTE: only the attitude update and specific force frame transformation
19955 * phases are precise.
19956 *
19957 * @param timeInterval time interval between epochs expressed in seconds (s).
19958 * @param oldPosition previous curvilinear position expressed in terms of latitude,
19959 * longitude and height.
19960 * @param oldC previous body-to-NED coordinate transformation.
19961 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
19962 * along north, east and down axes.
19963 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
19964 * resolved along body-frame axes, averaged over time interval.
19965 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
19966 * resolved along body-frame axes, averaged over time interval.
19967 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
19968 * resolved along body-frame axes, averaged over time interval.
19969 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
19970 * resolved along body-frame axes, averaged over time interval and
19971 * expressed in radians per second (rad/s).
19972 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
19973 * resolved along body-frame axes, averaged over time interval and
19974 * expressed in radians per second (rad/s).
19975 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
19976 * resolved along body-frame axes, averaged over time interval and
19977 * expressed in radians per second (rad/s).
19978 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
19979 * @return estimated NED frame containing new body position, velocity and coordinate
19980 * transformation matrix.
19981 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
19982 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
19983 * body-to-NED-frame coordinate transformation matrix are
19984 * invalid.
19985 */
19986 public static NEDFrame navigateNEDAndReturnNew(
19987 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
19988 final NEDVelocity oldVelocity, final Acceleration fx, final Acceleration fy, final Acceleration fz,
19989 final double angularRateX, final double angularRateY, final double angularRateZ,
19990 final double accuracyThreshold) throws InertialNavigatorException,
19991 InvalidSourceAndDestinationFrameTypeException {
19992 final var result = new NEDFrame();
19993 navigateNED(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
19994 accuracyThreshold, result);
19995 return result;
19996 }
19997
19998 /**
19999 * Runs precision local-navigation-frame inertial navigation equations.
20000 * NOTE: only the attitude update and specific force frame transformation
20001 * phases are precise.
20002 *
20003 * @param timeInterval time interval between epochs expressed in seconds (s).
20004 * @param oldPosition previous curvilinear position expressed in terms of latitude,
20005 * longitude and height.
20006 * @param oldC previous body-to-NED coordinate transformation.
20007 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
20008 * along north, east and down axes.
20009 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
20010 * resolved along body-frame axes, averaged over time interval.
20011 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
20012 * resolved along body-frame axes, averaged over time interval.
20013 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
20014 * resolved along body-frame axes, averaged over time interval.
20015 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
20016 * resolved along body-frame axes, averaged over time interval and
20017 * expressed in radians per second (rad/s).
20018 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
20019 * resolved along body-frame axes, averaged over time interval and
20020 * expressed in radians per second (rad/s).
20021 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
20022 * resolved along body-frame axes, averaged over time interval and
20023 * expressed in radians per second (rad/s).
20024 * @return estimated NED frame containing new body position, velocity and coordinate
20025 * transformation matrix.
20026 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
20027 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
20028 * body-to-NED-frame coordinate transformation matrix are
20029 * invalid.
20030 */
20031 public static NEDFrame navigateNEDAndReturnNew(
20032 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
20033 final NEDVelocity oldVelocity, final Acceleration fx, final Acceleration fy, final Acceleration fz,
20034 final double angularRateX, final double angularRateY, final double angularRateZ)
20035 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
20036 return navigateNEDAndReturnNew(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz,
20037 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
20038 }
20039
20040 /**
20041 * Runs precision local-navigation-frame inertial navigation equations.
20042 * NOTE: only the attitude update and specific force frame transformation
20043 * phases are precise.
20044 *
20045 * @param timeInterval time interval between epochs.
20046 * @param oldPosition previous curvilinear position expressed in terms of latitude,
20047 * longitude and height.
20048 * @param oldC previous body-to-NED coordinate transformation.
20049 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
20050 * along north, east and down axes.
20051 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
20052 * resolved along body-frame axes, averaged over time interval.
20053 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
20054 * resolved along body-frame axes, averaged over time interval.
20055 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
20056 * resolved along body-frame axes, averaged over time interval.
20057 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
20058 * resolved along body-frame axes, averaged over time interval and
20059 * expressed in radians per second (rad/s).
20060 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
20061 * resolved along body-frame axes, averaged over time interval and
20062 * expressed in radians per second (rad/s).
20063 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
20064 * resolved along body-frame axes, averaged over time interval and
20065 * expressed in radians per second (rad/s).
20066 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
20067 * @return estimated NED frame containing new body position, velocity and coordinate
20068 * transformation matrix.
20069 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
20070 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
20071 * body-to-NED-frame coordinate transformation matrix are
20072 * invalid.
20073 */
20074 public static NEDFrame navigateNEDAndReturnNew(
20075 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
20076 final NEDVelocity oldVelocity, final Acceleration fx, final Acceleration fy, final Acceleration fz,
20077 final double angularRateX, final double angularRateY, final double angularRateZ,
20078 final double accuracyThreshold) throws InertialNavigatorException,
20079 InvalidSourceAndDestinationFrameTypeException {
20080 final var result = new NEDFrame();
20081 navigateNED(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
20082 accuracyThreshold, result);
20083 return result;
20084 }
20085
20086 /**
20087 * Runs precision local-navigation-frame inertial navigation equations.
20088 * NOTE: only the attitude update and specific force frame transformation
20089 * phases are precise.
20090 *
20091 * @param timeInterval time interval between epochs.
20092 * @param oldPosition previous curvilinear position expressed in terms of latitude,
20093 * longitude and height.
20094 * @param oldC previous body-to-NED coordinate transformation.
20095 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
20096 * along north, east and down axes.
20097 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
20098 * resolved along body-frame axes, averaged over time interval.
20099 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
20100 * resolved along body-frame axes, averaged over time interval.
20101 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
20102 * resolved along body-frame axes, averaged over time interval.
20103 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
20104 * resolved along body-frame axes, averaged over time interval and
20105 * expressed in radians per second (rad/s).
20106 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
20107 * resolved along body-frame axes, averaged over time interval and
20108 * expressed in radians per second (rad/s).
20109 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
20110 * resolved along body-frame axes, averaged over time interval and
20111 * expressed in radians per second (rad/s).
20112 * @return estimated NED frame containing new body position, velocity and coordinate
20113 * transformation matrix.
20114 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
20115 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
20116 * body-to-NED-frame coordinate transformation matrix are
20117 * invalid.
20118 */
20119 public static NEDFrame navigateNEDAndReturnNew(
20120 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
20121 final NEDVelocity oldVelocity, final Acceleration fx, final Acceleration fy, final Acceleration fz,
20122 final double angularRateX, final double angularRateY, final double angularRateZ)
20123 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
20124 return navigateNEDAndReturnNew(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz,
20125 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
20126 }
20127
20128 /**
20129 * Runs precision local-navigation-frame inertial navigation equations.
20130 * NOTE: only the attitude update and specific force frame transformation
20131 * phases are precise.
20132 *
20133 * @param timeInterval time interval between epochs expressed in seconds (s).
20134 * @param oldLatitude previous latitude expressed in radians (rad).
20135 * @param oldLongitude previous longitude expressed in radians (rad).
20136 * @param oldHeight previous height expressed in meters (m).
20137 * @param oldC previous body-to-NED coordinate transformation.
20138 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
20139 * resolved along NED-frame axes and expressed in meters per second (m/s).
20140 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
20141 * resolved along NED-frame axes and expressed in meters per second (m/s).
20142 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
20143 * resolved along NED-frame axes and expressed in meters per second (m/s).
20144 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
20145 * resolved along body-frame axes, averaged over time interval and
20146 * expressed in meters per squared second (m/s^2).
20147 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
20148 * resolved along body-frame axes, averaged over time interval and
20149 * expressed in meters per squared second (m/s^2).
20150 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
20151 * resolved along body-frame axes, averaged over time interval and
20152 * expressed in meters per squared second (m/s^2).
20153 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
20154 * resolved along body-frame axes, averaged over time interval.
20155 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
20156 * resolved along body-frame axes, averaged over time interval.
20157 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
20158 * resolved along body-frame axes, averaged over time interval.
20159 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
20160 * @return estimated NED frame containing new body position, velocity and coordinate
20161 * transformation matrix.
20162 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
20163 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
20164 * body-to-NED-frame coordinate transformation matrix are
20165 * invalid.
20166 */
20167 public static NEDFrame navigateNEDAndReturnNew(
20168 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
20169 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
20170 final double fx, final double fy, final double fz,
20171 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
20172 final double accuracyThreshold) throws InertialNavigatorException,
20173 InvalidSourceAndDestinationFrameTypeException {
20174 final var result = new NEDFrame();
20175 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
20176 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
20177 return result;
20178 }
20179
20180 /**
20181 * Runs precision local-navigation-frame inertial navigation equations.
20182 * NOTE: only the attitude update and specific force frame transformation
20183 * phases are precise.
20184 *
20185 * @param timeInterval time interval between epochs.
20186 * @param oldLatitude previous latitude expressed in radians (rad).
20187 * @param oldLongitude previous longitude expressed in radians (rad).
20188 * @param oldHeight previous height expressed in meters (m).
20189 * @param oldC previous body-to-NED coordinate transformation.
20190 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
20191 * resolved along NED-frame axes and expressed in meters per second (m/s).
20192 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
20193 * resolved along NED-frame axes and expressed in meters per second (m/s).
20194 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
20195 * resolved along NED-frame axes and expressed in meters per second (m/s).
20196 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
20197 * resolved along body-frame axes, averaged over time interval and
20198 * expressed in meters per squared second (m/s^2).
20199 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
20200 * resolved along body-frame axes, averaged over time interval and
20201 * expressed in meters per squared second (m/s^2).
20202 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
20203 * resolved along body-frame axes, averaged over time interval and
20204 * expressed in meters per squared second (m/s^2).
20205 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
20206 * resolved along body-frame axes, averaged over time interval.
20207 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
20208 * resolved along body-frame axes, averaged over time interval.
20209 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
20210 * resolved along body-frame axes, averaged over time interval.
20211 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
20212 * @return estimated NED frame containing new body position, velocity and coordinate
20213 * transformation matrix.
20214 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
20215 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
20216 * body-to-NED-frame coordinate transformation matrix are
20217 * invalid.
20218 */
20219 public static NEDFrame navigateNEDAndReturnNew(
20220 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
20221 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
20222 final double fx, final double fy, final double fz,
20223 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
20224 final double accuracyThreshold) throws InertialNavigatorException,
20225 InvalidSourceAndDestinationFrameTypeException {
20226 final var result = new NEDFrame();
20227 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
20228 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
20229 return result;
20230 }
20231
20232 /**
20233 * Runs precision local-navigation-frame inertial navigation equations.
20234 * NOTE: only the attitude update and specific force frame transformation
20235 * phases are precise.
20236 *
20237 * @param timeInterval time interval between epochs expressed in seconds (s).
20238 * @param oldPosition previous curvilinear position expressed in terms of latitude,
20239 * longitude and height.
20240 * @param oldC previous body-to-NED coordinate transformation.
20241 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
20242 * resolved along NED-frame axes and expressed in meters per second (m/s).
20243 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
20244 * resolved along NED-frame axes and expressed in meters per second (m/s).
20245 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
20246 * resolved along NED-frame axes and expressed in meters per second (m/s).
20247 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
20248 * resolved along body-frame axes, averaged over time interval and
20249 * expressed in meters per squared second (m/s^2).
20250 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
20251 * resolved along body-frame axes, averaged over time interval and
20252 * expressed in meters per squared second (m/s^2).
20253 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
20254 * resolved along body-frame axes, averaged over time interval and
20255 * expressed in meters per squared second (m/s^2).
20256 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
20257 * resolved along body-frame axes, averaged over time interval.
20258 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
20259 * resolved along body-frame axes, averaged over time interval.
20260 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
20261 * resolved along body-frame axes, averaged over time interval.
20262 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
20263 * @return estimated NED frame containing new body position, velocity and coordinate
20264 * transformation matrix.
20265 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
20266 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
20267 * body-to-NED-frame coordinate transformation matrix are
20268 * invalid.
20269 */
20270 public static NEDFrame navigateNEDAndReturnNew(
20271 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
20272 final double oldVn, final double oldVe, final double oldVd,
20273 final double fx, final double fy, final double fz,
20274 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
20275 final double accuracyThreshold) throws InertialNavigatorException,
20276 InvalidSourceAndDestinationFrameTypeException {
20277 final var result = new NEDFrame();
20278 navigateNED(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
20279 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
20280 return result;
20281 }
20282
20283 /**
20284 * Runs precision local-navigation-frame inertial navigation equations.
20285 * NOTE: only the attitude update and specific force frame transformation
20286 * phases are precise.
20287 *
20288 * @param timeInterval time interval between epochs.
20289 * @param oldPosition previous curvilinear position expressed in terms of latitude,
20290 * longitude and height.
20291 * @param oldC previous body-to-NED coordinate transformation.
20292 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
20293 * resolved along NED-frame axes and expressed in meters per second (m/s).
20294 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
20295 * resolved along NED-frame axes and expressed in meters per second (m/s).
20296 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
20297 * resolved along NED-frame axes and expressed in meters per second (m/s).
20298 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
20299 * resolved along body-frame axes, averaged over time interval and
20300 * expressed in meters per squared second (m/s^2).
20301 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
20302 * resolved along body-frame axes, averaged over time interval and
20303 * expressed in meters per squared second (m/s^2).
20304 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
20305 * resolved along body-frame axes, averaged over time interval and
20306 * expressed in meters per squared second (m/s^2).
20307 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
20308 * resolved along body-frame axes, averaged over time interval.
20309 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
20310 * resolved along body-frame axes, averaged over time interval.
20311 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
20312 * resolved along body-frame axes, averaged over time interval.
20313 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
20314 * @return estimated NED frame containing new body position, velocity and coordinate
20315 * transformation matrix.
20316 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
20317 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
20318 * body-to-NED-frame coordinate transformation matrix are
20319 * invalid.
20320 */
20321 public static NEDFrame navigateNEDAndReturnNew(
20322 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
20323 final double oldVn, final double oldVe, final double oldVd,
20324 final double fx, final double fy, final double fz,
20325 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
20326 final double accuracyThreshold) throws InertialNavigatorException,
20327 InvalidSourceAndDestinationFrameTypeException {
20328 final var result = new NEDFrame();
20329 navigateNED(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
20330 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
20331 return result;
20332 }
20333
20334 /**
20335 * Runs precision local-navigation-frame inertial navigation equations.
20336 * NOTE: only the attitude update and specific force frame transformation
20337 * phases are precise.
20338 *
20339 * @param timeInterval time interval between epochs expressed in seconds (s).
20340 * @param oldLatitude previous latitude expressed in radians (rad).
20341 * @param oldLongitude previous longitude expressed in radians (rad).
20342 * @param oldHeight previous height expressed in meters (m).
20343 * @param oldC previous body-to-NED coordinate transformation.
20344 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
20345 * along north, east and down axes.
20346 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
20347 * resolved along body-frame axes, averaged over time interval and
20348 * expressed in meters per squared second (m/s^2).
20349 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
20350 * resolved along body-frame axes, averaged over time interval and
20351 * expressed in meters per squared second (m/s^2).
20352 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
20353 * resolved along body-frame axes, averaged over time interval and
20354 * expressed in meters per squared second (m/s^2).
20355 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
20356 * resolved along body-frame axes, averaged over time interval.
20357 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
20358 * resolved along body-frame axes, averaged over time interval.
20359 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
20360 * resolved along body-frame axes, averaged over time interval.
20361 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
20362 * @return estimated NED frame containing new body position, velocity and coordinate
20363 * transformation matrix.
20364 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
20365 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
20366 * body-to-NED-frame coordinate transformation matrix are
20367 * invalid.
20368 */
20369 public static NEDFrame navigateNEDAndReturnNew(
20370 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
20371 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
20372 final double fx, final double fy, final double fz,
20373 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
20374 final double accuracyThreshold) throws InertialNavigatorException,
20375 InvalidSourceAndDestinationFrameTypeException {
20376 final var result = new NEDFrame();
20377 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
20378 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
20379 return result;
20380 }
20381
20382 /**
20383 * Runs precision local-navigation-frame inertial navigation equations.
20384 * NOTE: only the attitude update and specific force frame transformation
20385 * phases are precise.
20386 *
20387 * @param timeInterval time interval between epochs.
20388 * @param oldLatitude previous latitude expressed in radians (rad).
20389 * @param oldLongitude previous longitude expressed in radians (rad).
20390 * @param oldHeight previous height expressed in meters (m).
20391 * @param oldC previous body-to-NED coordinate transformation.
20392 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
20393 * along north, east and down axes.
20394 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
20395 * resolved along body-frame axes, averaged over time interval and
20396 * expressed in meters per squared second (m/s^2).
20397 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
20398 * resolved along body-frame axes, averaged over time interval and
20399 * expressed in meters per squared second (m/s^2).
20400 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
20401 * resolved along body-frame axes, averaged over time interval and
20402 * expressed in meters per squared second (m/s^2).
20403 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
20404 * resolved along body-frame axes, averaged over time interval.
20405 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
20406 * resolved along body-frame axes, averaged over time interval.
20407 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
20408 * resolved along body-frame axes, averaged over time interval.
20409 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
20410 * @return estimated NED frame containing new body position, velocity and coordinate
20411 * transformation matrix.
20412 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
20413 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
20414 * body-to-NED-frame coordinate transformation matrix are
20415 * invalid.
20416 */
20417 public static NEDFrame navigateNEDAndReturnNew(
20418 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
20419 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
20420 final double fx, final double fy, final double fz,
20421 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
20422 final double accuracyThreshold) throws InertialNavigatorException,
20423 InvalidSourceAndDestinationFrameTypeException {
20424 final var result = new NEDFrame();
20425 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
20426 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
20427 return result;
20428 }
20429
20430 /**
20431 * Runs precision local-navigation-frame inertial navigation equations.
20432 * NOTE: only the attitude update and specific force frame transformation
20433 * phases are precise.
20434 *
20435 * @param timeInterval time interval between epochs expressed in seconds (s).
20436 * @param oldPosition previous curvilinear position expressed in terms of latitude,
20437 * longitude and height.
20438 * @param oldC previous body-to-NED coordinate transformation.
20439 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
20440 * along north, east and down axes.
20441 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
20442 * resolved along body-frame axes, averaged over time interval and
20443 * expressed in meters per squared second (m/s^2).
20444 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
20445 * resolved along body-frame axes, averaged over time interval and
20446 * expressed in meters per squared second (m/s^2).
20447 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
20448 * resolved along body-frame axes, averaged over time interval and
20449 * expressed in meters per squared second (m/s^2).
20450 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
20451 * resolved along body-frame axes, averaged over time interval.
20452 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
20453 * resolved along body-frame axes, averaged over time interval.
20454 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
20455 * resolved along body-frame axes, averaged over time interval.
20456 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
20457 * @return estimated NED frame containing new body position, velocity and coordinate
20458 * transformation matrix.
20459 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
20460 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
20461 * body-to-NED-frame coordinate transformation matrix are
20462 * invalid.
20463 */
20464 public static NEDFrame navigateNEDAndReturnNew(
20465 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
20466 final NEDVelocity oldVelocity, final double fx, final double fy, final double fz,
20467 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
20468 final double accuracyThreshold) throws InertialNavigatorException,
20469 InvalidSourceAndDestinationFrameTypeException {
20470 final var result = new NEDFrame();
20471 navigateNED(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
20472 accuracyThreshold, result);
20473 return result;
20474 }
20475
20476 /**
20477 * Runs precision local-navigation-frame inertial navigation equations.
20478 * NOTE: only the attitude update and specific force frame transformation
20479 * phases are precise.
20480 *
20481 * @param timeInterval time interval between epochs expressed in seconds (s).
20482 * @param oldPosition previous curvilinear position expressed in terms of latitude,
20483 * longitude and height.
20484 * @param oldC previous body-to-NED coordinate transformation.
20485 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
20486 * along north, east and down axes.
20487 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
20488 * resolved along body-frame axes, averaged over time interval and
20489 * expressed in meters per squared second (m/s^2).
20490 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
20491 * resolved along body-frame axes, averaged over time interval and
20492 * expressed in meters per squared second (m/s^2).
20493 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
20494 * resolved along body-frame axes, averaged over time interval and
20495 * expressed in meters per squared second (m/s^2).
20496 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
20497 * resolved along body-frame axes, averaged over time interval.
20498 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
20499 * resolved along body-frame axes, averaged over time interval.
20500 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
20501 * resolved along body-frame axes, averaged over time interval.
20502 * @return estimated NED frame containing new body position, velocity and coordinate
20503 * transformation matrix.
20504 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
20505 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
20506 * body-to-NED-frame coordinate transformation matrix are
20507 * invalid.
20508 */
20509 public static NEDFrame navigateNEDAndReturnNew(
20510 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
20511 final NEDVelocity oldVelocity, final double fx, final double fy, final double fz,
20512 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
20513 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
20514 return navigateNEDAndReturnNew(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz,
20515 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
20516 }
20517
20518 /**
20519 * Runs precision local-navigation-frame inertial navigation equations.
20520 * NOTE: only the attitude update and specific force frame transformation
20521 * phases are precise.
20522 *
20523 * @param timeInterval time interval between epochs.
20524 * @param oldPosition previous curvilinear position expressed in terms of latitude,
20525 * longitude and height.
20526 * @param oldC previous body-to-NED coordinate transformation.
20527 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
20528 * along north, east and down axes.
20529 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
20530 * resolved along body-frame axes, averaged over time interval and
20531 * expressed in meters per squared second (m/s^2).
20532 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
20533 * resolved along body-frame axes, averaged over time interval and
20534 * expressed in meters per squared second (m/s^2).
20535 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
20536 * resolved along body-frame axes, averaged over time interval and
20537 * expressed in meters per squared second (m/s^2).
20538 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
20539 * resolved along body-frame axes, averaged over time interval.
20540 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
20541 * resolved along body-frame axes, averaged over time interval.
20542 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
20543 * resolved along body-frame axes, averaged over time interval.
20544 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
20545 * @return estimated NED frame containing new body position, velocity and coordinate
20546 * transformation matrix.
20547 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
20548 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
20549 * body-to-NED-frame coordinate transformation matrix are
20550 * invalid.
20551 */
20552 public static NEDFrame navigateNEDAndReturnNew(
20553 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
20554 final NEDVelocity oldVelocity, final double fx, final double fy, final double fz,
20555 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
20556 final double accuracyThreshold) throws InertialNavigatorException,
20557 InvalidSourceAndDestinationFrameTypeException {
20558 final var result = new NEDFrame();
20559 navigateNED(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
20560 accuracyThreshold, result);
20561 return result;
20562 }
20563
20564 /**
20565 * Runs precision local-navigation-frame inertial navigation equations.
20566 * NOTE: only the attitude update and specific force frame transformation
20567 * phases are precise.
20568 *
20569 * @param timeInterval time interval between epochs.
20570 * @param oldPosition previous curvilinear position expressed in terms of latitude,
20571 * longitude and height.
20572 * @param oldC previous body-to-NED coordinate transformation.
20573 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
20574 * along north, east and down axes.
20575 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
20576 * resolved along body-frame axes, averaged over time interval and
20577 * expressed in meters per squared second (m/s^2).
20578 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
20579 * resolved along body-frame axes, averaged over time interval and
20580 * expressed in meters per squared second (m/s^2).
20581 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
20582 * resolved along body-frame axes, averaged over time interval and
20583 * expressed in meters per squared second (m/s^2).
20584 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
20585 * resolved along body-frame axes, averaged over time interval.
20586 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
20587 * resolved along body-frame axes, averaged over time interval.
20588 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
20589 * resolved along body-frame axes, averaged over time interval.
20590 * @return estimated NED frame containing new body position, velocity and coordinate
20591 * transformation matrix.
20592 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
20593 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
20594 * body-to-NED-frame coordinate transformation matrix are
20595 * invalid.
20596 */
20597 public static NEDFrame navigateNEDAndReturnNew(
20598 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
20599 final NEDVelocity oldVelocity, final double fx, final double fy, final double fz,
20600 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
20601 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
20602 return navigateNEDAndReturnNew(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz,
20603 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
20604 }
20605
20606 /**
20607 * Runs precision local-navigation-frame inertial navigation equations.
20608 * NOTE: only the attitude update and specific force frame transformation
20609 * phases are precise.
20610 *
20611 * @param timeInterval time interval between epochs expressed in seconds (s).
20612 * @param oldLatitude previous latitude angle.
20613 * @param oldLongitude previous longitude angle.
20614 * @param oldHeight previous height.
20615 * @param oldC previous body-to-NED coordinate transformation.
20616 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
20617 * resolved along NED-frame axes.
20618 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
20619 * resolved along NED-frame axes.
20620 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
20621 * resolved along NED-frame axes.
20622 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
20623 * resolved along body-frame axes, averaged over time interval and
20624 * expressed in meters per squared second (m/s^2).
20625 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
20626 * resolved along body-frame axes, averaged over time interval and
20627 * expressed in meters per squared second (m/s^2).
20628 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
20629 * resolved along body-frame axes, averaged over time interval and
20630 * expressed in meters per squared second (m/s^2).
20631 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
20632 * resolved along body-frame axes, averaged over time interval and
20633 * expressed in radians per second (rad/s).
20634 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
20635 * resolved along body-frame axes, averaged over time interval and
20636 * expressed in radians per second (rad/s).
20637 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
20638 * resolved along body-frame axes, averaged over time interval and
20639 * expressed in radians per second (rad/s).
20640 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
20641 * @return estimated NED frame containing new body position, velocity and coordinate
20642 * transformation matrix.
20643 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
20644 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
20645 * body-to-NED-frame coordinate transformation matrix are
20646 * invalid.
20647 */
20648 public static NEDFrame navigateNEDAndReturnNew(
20649 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
20650 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
20651 final double fx, final double fy, final double fz,
20652 final double angularRateX, final double angularRateY, final double angularRateZ,
20653 final double accuracyThreshold) throws InertialNavigatorException,
20654 InvalidSourceAndDestinationFrameTypeException {
20655 final var result = new NEDFrame();
20656 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
20657 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
20658 return result;
20659 }
20660
20661 /**
20662 * Runs precision local-navigation-frame inertial navigation equations.
20663 * NOTE: only the attitude update and specific force frame transformation
20664 * phases are precise.
20665 *
20666 * @param timeInterval time interval between epochs.
20667 * @param oldLatitude previous latitude angle.
20668 * @param oldLongitude previous longitude angle.
20669 * @param oldHeight previous height.
20670 * @param oldC previous body-to-NED coordinate transformation.
20671 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
20672 * resolved along NED-frame axes.
20673 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
20674 * resolved along NED-frame axes.
20675 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
20676 * resolved along NED-frame axes.
20677 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
20678 * resolved along body-frame axes, averaged over time interval and
20679 * expressed in meters per squared second (m/s^2).
20680 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
20681 * resolved along body-frame axes, averaged over time interval and
20682 * expressed in meters per squared second (m/s^2).
20683 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
20684 * resolved along body-frame axes, averaged over time interval and
20685 * expressed in meters per squared second (m/s^2).
20686 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
20687 * resolved along body-frame axes, averaged over time interval and
20688 * expressed in radians per second (rad/s).
20689 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
20690 * resolved along body-frame axes, averaged over time interval and
20691 * expressed in radians per second (rad/s).
20692 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
20693 * resolved along body-frame axes, averaged over time interval and
20694 * expressed in radians per second (rad/s).
20695 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
20696 * @return estimated NED frame containing new body position, velocity and coordinate
20697 * transformation matrix.
20698 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
20699 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
20700 * body-to-NED-frame coordinate transformation matrix are
20701 * invalid.
20702 */
20703 public static NEDFrame navigateNEDAndReturnNew(
20704 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
20705 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
20706 final double fx, final double fy, final double fz,
20707 final double angularRateX, final double angularRateY, final double angularRateZ,
20708 final double accuracyThreshold) throws InertialNavigatorException,
20709 InvalidSourceAndDestinationFrameTypeException {
20710 final var result = new NEDFrame();
20711 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
20712 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
20713 return result;
20714 }
20715
20716 /**
20717 * Runs precision local-navigation-frame inertial navigation equations.
20718 * NOTE: only the attitude update and specific force frame transformation
20719 * phases are precise.
20720 *
20721 * @param timeInterval time interval between epochs expressed in seconds (s).
20722 * @param oldLatitude previous latitude angle.
20723 * @param oldLongitude previous longitude angle.
20724 * @param oldHeight previous height.
20725 * @param oldC previous body-to-NED coordinate transformation.
20726 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
20727 * resolved along NED-frame axes.
20728 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
20729 * resolved along NED-frame axes.
20730 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
20731 * resolved along NED-frame axes.
20732 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
20733 * resolved along body-frame axes, averaged over time interval.
20734 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
20735 * resolved along body-frame axes, averaged over time interval.
20736 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
20737 * resolved along body-frame axes, averaged over time interval.
20738 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
20739 * resolved along body-frame axes, averaged over time interval.
20740 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
20741 * resolved along body-frame axes, averaged over time interval.
20742 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
20743 * resolved along body-frame axes, averaged over time interval.
20744 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
20745 * @return estimated NED frame containing new body position, velocity and coordinate
20746 * transformation matrix.
20747 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
20748 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
20749 * body-to-NED-frame coordinate transformation matrix are
20750 * invalid.
20751 */
20752 public static NEDFrame navigateNEDAndReturnNew(
20753 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
20754 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
20755 final Acceleration fx, final Acceleration fy, final Acceleration fz,
20756 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
20757 final double accuracyThreshold) throws InertialNavigatorException,
20758 InvalidSourceAndDestinationFrameTypeException {
20759 final var result = new NEDFrame();
20760 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
20761 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
20762 return result;
20763 }
20764
20765 /**
20766 * Runs precision local-navigation-frame inertial navigation equations.
20767 * NOTE: only the attitude update and specific force frame transformation
20768 * phases are precise.
20769 *
20770 * @param timeInterval time interval between epochs.
20771 * @param oldLatitude previous latitude angle.
20772 * @param oldLongitude previous longitude angle.
20773 * @param oldHeight previous height.
20774 * @param oldC previous body-to-NED coordinate transformation.
20775 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
20776 * resolved along NED-frame axes.
20777 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
20778 * resolved along NED-frame axes.
20779 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
20780 * resolved along NED-frame axes.
20781 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
20782 * resolved along body-frame axes, averaged over time interval.
20783 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
20784 * resolved along body-frame axes, averaged over time interval.
20785 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
20786 * resolved along body-frame axes, averaged over time interval.
20787 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
20788 * resolved along body-frame axes, averaged over time interval.
20789 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
20790 * resolved along body-frame axes, averaged over time interval.
20791 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
20792 * resolved along body-frame axes, averaged over time interval.
20793 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
20794 * @return estimated NED frame containing new body position, velocity and coordinate
20795 * transformation matrix.
20796 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
20797 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
20798 * body-to-NED-frame coordinate transformation matrix are
20799 * invalid.
20800 */
20801 public static NEDFrame navigateNEDAndReturnNew(
20802 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
20803 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
20804 final Acceleration fx, final Acceleration fy, final Acceleration fz,
20805 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
20806 final double accuracyThreshold) throws InertialNavigatorException,
20807 InvalidSourceAndDestinationFrameTypeException {
20808 final var result = new NEDFrame();
20809 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
20810 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
20811 return result;
20812 }
20813
20814 /**
20815 * Runs precision local-navigation-frame inertial navigation equations.
20816 * NOTE: only the attitude update and specific force frame transformation
20817 * phases are precise.
20818 *
20819 * @param timeInterval time interval between epochs expressed in seconds (s).
20820 * @param oldLatitude previous latitude expressed in radians (rad).
20821 * @param oldLongitude previous longitude expressed in radians (rad).
20822 * @param oldHeight previous height expressed in meters (m).
20823 * @param oldC previous body-to-NED coordinate transformation.
20824 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
20825 * resolved along NED-frame axes.
20826 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
20827 * resolved along NED-frame axes.
20828 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
20829 * resolved along NED-frame axes.
20830 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
20831 * resolved along body-frame axes, averaged over time interval.
20832 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
20833 * resolved along body-frame axes, averaged over time interval.
20834 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
20835 * resolved along body-frame axes, averaged over time interval.
20836 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
20837 * resolved along body-frame axes, averaged over time interval.
20838 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
20839 * resolved along body-frame axes, averaged over time interval.
20840 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
20841 * resolved along body-frame axes, averaged over time interval.
20842 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
20843 * @return estimated NED frame containing new body position, velocity and coordinate
20844 * transformation matrix.
20845 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
20846 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
20847 * body-to-NED-frame coordinate transformation matrix are
20848 * invalid.
20849 */
20850 public static NEDFrame navigateNEDAndReturnNew(
20851 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
20852 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
20853 final Acceleration fx, final Acceleration fy, final Acceleration fz,
20854 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
20855 final double accuracyThreshold) throws InertialNavigatorException,
20856 InvalidSourceAndDestinationFrameTypeException {
20857 final var result = new NEDFrame();
20858 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
20859 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
20860 return result;
20861 }
20862
20863 /**
20864 * Runs precision local-navigation-frame inertial navigation equations.
20865 * NOTE: only the attitude update and specific force frame transformation
20866 * phases are precise.
20867 *
20868 * @param timeInterval time interval between epochs.
20869 * @param oldLatitude previous latitude expressed in radians (rad).
20870 * @param oldLongitude previous longitude expressed in radians (rad).
20871 * @param oldHeight previous height expressed in meters (m).
20872 * @param oldC previous body-to-NED coordinate transformation.
20873 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
20874 * resolved along NED-frame axes.
20875 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
20876 * resolved along NED-frame axes.
20877 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
20878 * resolved along NED-frame axes.
20879 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
20880 * resolved along body-frame axes, averaged over time interval.
20881 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
20882 * resolved along body-frame axes, averaged over time interval.
20883 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
20884 * resolved along body-frame axes, averaged over time interval.
20885 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
20886 * resolved along body-frame axes, averaged over time interval.
20887 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
20888 * resolved along body-frame axes, averaged over time interval.
20889 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
20890 * resolved along body-frame axes, averaged over time interval.
20891 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
20892 * @return estimated NED frame containing new body position, velocity and coordinate
20893 * transformation matrix.
20894 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
20895 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
20896 * body-to-NED-frame coordinate transformation matrix are
20897 * invalid.
20898 */
20899 public static NEDFrame navigateNEDAndReturnNew(
20900 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
20901 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
20902 final Acceleration fx, final Acceleration fy, final Acceleration fz,
20903 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
20904 final double accuracyThreshold) throws InertialNavigatorException,
20905 InvalidSourceAndDestinationFrameTypeException {
20906 final var result = new NEDFrame();
20907 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
20908 fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
20909 return result;
20910 }
20911
20912 /**
20913 * Runs precision local-navigation-frame inertial navigation equations.
20914 * NOTE: only the attitude update and specific force frame transformation
20915 * phases are precise.
20916 *
20917 * @param timeInterval time interval between epochs expressed in seconds (s).
20918 * @param oldPosition previous curvilinear position expressed in terms of latitude,
20919 * longitude and height.
20920 * @param oldC previous body-to-NED coordinate transformation.
20921 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
20922 * resolved along NED-frame axes.
20923 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
20924 * resolved along NED-frame axes.
20925 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
20926 * resolved along NED-frame axes.
20927 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
20928 * resolved along body-frame axes, averaged over time interval.
20929 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
20930 * resolved along body-frame axes, averaged over time interval.
20931 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
20932 * resolved along body-frame axes, averaged over time interval.
20933 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
20934 * resolved along body-frame axes, averaged over time interval.
20935 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
20936 * resolved along body-frame axes, averaged over time interval.
20937 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
20938 * resolved along body-frame axes, averaged over time interval.
20939 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
20940 * @return estimated NED frame containing new body position, velocity and coordinate
20941 * transformation matrix.
20942 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
20943 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
20944 * body-to-NED-frame coordinate transformation matrix are
20945 * invalid.
20946 */
20947 public static NEDFrame navigateNEDAndReturnNew(
20948 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
20949 final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
20950 final Acceleration fx, final Acceleration fy, final Acceleration fz,
20951 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
20952 final double accuracyThreshold) throws InertialNavigatorException,
20953 InvalidSourceAndDestinationFrameTypeException {
20954 final var result = new NEDFrame();
20955 navigateNED(timeInterval, oldPosition, oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
20956 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
20957 return result;
20958 }
20959
20960 /**
20961 * Runs precision local-navigation-frame inertial navigation equations.
20962 * NOTE: only the attitude update and specific force frame transformation
20963 * phases are precise.
20964 *
20965 * @param timeInterval time interval between epochs.
20966 * @param oldPosition previous curvilinear position expressed in terms of latitude,
20967 * longitude and height.
20968 * @param oldC previous body-to-NED coordinate transformation.
20969 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
20970 * resolved along NED-frame axes.
20971 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
20972 * resolved along NED-frame axes.
20973 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
20974 * resolved along NED-frame axes.
20975 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
20976 * resolved along body-frame axes, averaged over time interval.
20977 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
20978 * resolved along body-frame axes, averaged over time interval.
20979 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
20980 * resolved along body-frame axes, averaged over time interval.
20981 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
20982 * resolved along body-frame axes, averaged over time interval.
20983 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
20984 * resolved along body-frame axes, averaged over time interval.
20985 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
20986 * resolved along body-frame axes, averaged over time interval.
20987 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
20988 * @return estimated NED frame containing new body position, velocity and coordinate
20989 * transformation matrix.
20990 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
20991 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
20992 * body-to-NED-frame coordinate transformation matrix are
20993 * invalid.
20994 */
20995 public static NEDFrame navigateNEDAndReturnNew(
20996 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
20997 final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
20998 final Acceleration fx, final Acceleration fy, final Acceleration fz,
20999 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
21000 final double accuracyThreshold) throws InertialNavigatorException,
21001 InvalidSourceAndDestinationFrameTypeException {
21002 final var result = new NEDFrame();
21003 navigateNED(timeInterval, oldPosition, oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
21004 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
21005 return result;
21006 }
21007
21008 /**
21009 * Runs precision local-navigation-frame inertial navigation equations.
21010 * NOTE: only the attitude update and specific force frame transformation
21011 * phases are precise.
21012 *
21013 * @param timeInterval time interval between epochs expressed in seconds (s).
21014 * @param oldLatitude previous latitude angle.
21015 * @param oldLongitude previous longitude angle.
21016 * @param oldHeight previous height.
21017 * @param oldC previous body-to-NED coordinate transformation.
21018 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
21019 * along north, east and down axes.
21020 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
21021 * resolved along body-frame axes, averaged over time interval.
21022 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
21023 * resolved along body-frame axes, averaged over time interval.
21024 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
21025 * resolved along body-frame axes, averaged over time interval.
21026 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
21027 * resolved along body-frame axes, averaged over time interval.
21028 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
21029 * resolved along body-frame axes, averaged over time interval.
21030 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
21031 * resolved along body-frame axes, averaged over time interval.
21032 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
21033 * @return estimated NED frame containing new body position, velocity and coordinate
21034 * transformation matrix.
21035 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
21036 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
21037 * body-to-NED-frame coordinate transformation matrix are
21038 * invalid.
21039 */
21040 public static NEDFrame navigateNEDAndReturnNew(
21041 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
21042 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
21043 final Acceleration fx, final Acceleration fy, final Acceleration fz,
21044 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
21045 final double accuracyThreshold) throws InertialNavigatorException,
21046 InvalidSourceAndDestinationFrameTypeException {
21047 final var result = new NEDFrame();
21048 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
21049 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
21050 return result;
21051 }
21052
21053 /**
21054 * Runs precision local-navigation-frame inertial navigation equations.
21055 * NOTE: only the attitude update and specific force frame transformation
21056 * phases are precise.
21057 *
21058 * @param timeInterval time interval between epochs.
21059 * @param oldLatitude previous latitude angle.
21060 * @param oldLongitude previous longitude angle.
21061 * @param oldHeight previous height.
21062 * @param oldC previous body-to-NED coordinate transformation.
21063 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
21064 * along north, east and down axes.
21065 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
21066 * resolved along body-frame axes, averaged over time interval.
21067 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
21068 * resolved along body-frame axes, averaged over time interval.
21069 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
21070 * resolved along body-frame axes, averaged over time interval.
21071 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
21072 * resolved along body-frame axes, averaged over time interval.
21073 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
21074 * resolved along body-frame axes, averaged over time interval.
21075 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
21076 * resolved along body-frame axes, averaged over time interval.
21077 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
21078 * @return estimated NED frame containing new body position, velocity and coordinate
21079 * transformation matrix.
21080 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
21081 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
21082 * body-to-NED-frame coordinate transformation matrix are
21083 * invalid.
21084 */
21085 public static NEDFrame navigateNEDAndReturnNew(
21086 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
21087 final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
21088 final Acceleration fx, final Acceleration fy, final Acceleration fz,
21089 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
21090 final double accuracyThreshold) throws InertialNavigatorException,
21091 InvalidSourceAndDestinationFrameTypeException {
21092 final var result = new NEDFrame();
21093 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
21094 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
21095 return result;
21096 }
21097
21098 /**
21099 * Runs precision local-navigation-frame inertial navigation equations.
21100 * NOTE: only the attitude update and specific force frame transformation
21101 * phases are precise.
21102 *
21103 * @param timeInterval time interval between epochs expressed in seconds (s).
21104 * @param oldPosition previous curvilinear position expressed in terms of latitude,
21105 * longitude and height.
21106 * @param oldC previous body-to-NED coordinate transformation.
21107 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
21108 * along north, east and down axes.
21109 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
21110 * resolved along body-frame axes, averaged over time interval.
21111 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
21112 * resolved along body-frame axes, averaged over time interval.
21113 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
21114 * resolved along body-frame axes, averaged over time interval.
21115 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
21116 * resolved along body-frame axes, averaged over time interval.
21117 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
21118 * resolved along body-frame axes, averaged over time interval.
21119 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
21120 * resolved along body-frame axes, averaged over time interval.
21121 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
21122 * @return estimated NED frame containing new body position, velocity and coordinate
21123 * transformation matrix.
21124 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
21125 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
21126 * body-to-NED-frame coordinate transformation matrix are
21127 * invalid.
21128 */
21129 public static NEDFrame navigateNEDAndReturnNew(
21130 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
21131 final NEDVelocity oldVelocity, final Acceleration fx, final Acceleration fy, final Acceleration fz,
21132 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
21133 final double accuracyThreshold) throws InertialNavigatorException,
21134 InvalidSourceAndDestinationFrameTypeException {
21135 final var result = new NEDFrame();
21136 navigateNED(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
21137 accuracyThreshold, result);
21138 return result;
21139 }
21140
21141 /**
21142 * Runs precision local-navigation-frame inertial navigation equations.
21143 * NOTE: only the attitude update and specific force frame transformation
21144 * phases are precise.
21145 *
21146 * @param timeInterval time interval between epochs expressed in seconds (s).
21147 * @param oldPosition previous curvilinear position expressed in terms of latitude,
21148 * longitude and height.
21149 * @param oldC previous body-to-NED coordinate transformation.
21150 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
21151 * along north, east and down axes.
21152 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
21153 * resolved along body-frame axes, averaged over time interval.
21154 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
21155 * resolved along body-frame axes, averaged over time interval.
21156 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
21157 * resolved along body-frame axes, averaged over time interval.
21158 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
21159 * resolved along body-frame axes, averaged over time interval.
21160 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
21161 * resolved along body-frame axes, averaged over time interval.
21162 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
21163 * resolved along body-frame axes, averaged over time interval.
21164 * @return estimated NED frame containing new body position, velocity and coordinate
21165 * transformation matrix.
21166 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
21167 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
21168 * body-to-NED-frame coordinate transformation matrix are
21169 * invalid.
21170 */
21171 public static NEDFrame navigateNEDAndReturnNew(
21172 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
21173 final NEDVelocity oldVelocity, final Acceleration fx, final Acceleration fy, final Acceleration fz,
21174 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
21175 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
21176 return navigateNEDAndReturnNew(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz,
21177 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
21178 }
21179
21180 /**
21181 * Runs precision local-navigation-frame inertial navigation equations.
21182 * NOTE: only the attitude update and specific force frame transformation
21183 * phases are precise.
21184 *
21185 * @param timeInterval time interval between epochs.
21186 * @param oldPosition previous curvilinear position expressed in terms of latitude,
21187 * longitude and height.
21188 * @param oldC previous body-to-NED coordinate transformation.
21189 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
21190 * along north, east and down axes.
21191 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
21192 * resolved along body-frame axes, averaged over time interval.
21193 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
21194 * resolved along body-frame axes, averaged over time interval.
21195 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
21196 * resolved along body-frame axes, averaged over time interval.
21197 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
21198 * resolved along body-frame axes, averaged over time interval.
21199 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
21200 * resolved along body-frame axes, averaged over time interval.
21201 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
21202 * resolved along body-frame axes, averaged over time interval.
21203 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
21204 * @return estimated NED frame containing new body position, velocity and coordinate
21205 * transformation matrix.
21206 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
21207 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
21208 * body-to-NED-frame coordinate transformation matrix are
21209 * invalid.
21210 */
21211 public static NEDFrame navigateNEDAndReturnNew(
21212 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
21213 final NEDVelocity oldVelocity, final Acceleration fx, final Acceleration fy, final Acceleration fz,
21214 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
21215 final double accuracyThreshold) throws InertialNavigatorException,
21216 InvalidSourceAndDestinationFrameTypeException {
21217 final var result = new NEDFrame();
21218 navigateNED(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
21219 accuracyThreshold, result);
21220 return result;
21221 }
21222
21223 /**
21224 * Runs precision local-navigation-frame inertial navigation equations.
21225 * NOTE: only the attitude update and specific force frame transformation
21226 * phases are precise.
21227 *
21228 * @param timeInterval time interval between epochs.
21229 * @param oldPosition previous curvilinear position expressed in terms of latitude,
21230 * longitude and height.
21231 * @param oldC previous body-to-NED coordinate transformation.
21232 * @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
21233 * along north, east and down axes.
21234 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
21235 * resolved along body-frame axes, averaged over time interval.
21236 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
21237 * resolved along body-frame axes, averaged over time interval.
21238 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
21239 * resolved along body-frame axes, averaged over time interval.
21240 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
21241 * resolved along body-frame axes, averaged over time interval.
21242 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
21243 * resolved along body-frame axes, averaged over time interval.
21244 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
21245 * resolved along body-frame axes, averaged over time interval.
21246 * @return estimated NED frame containing new body position, velocity and coordinate
21247 * transformation matrix.
21248 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
21249 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
21250 * body-to-NED-frame coordinate transformation matrix are
21251 * invalid.
21252 */
21253 public static NEDFrame navigateNEDAndReturnNew(
21254 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
21255 final NEDVelocity oldVelocity, final Acceleration fx, final Acceleration fy, final Acceleration fz,
21256 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
21257 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
21258 return navigateNEDAndReturnNew(timeInterval, oldPosition, oldC, oldVelocity, fx, fy, fz,
21259 angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD);
21260 }
21261
21262 /**
21263 * Runs precision local-navigation-frame inertial navigation equations.
21264 * NOTE: only the attitude update and specific force frame transformation
21265 * phases are precise.
21266 *
21267 * @param timeInterval time interval between epochs expressed in seconds (s).
21268 * @param oldLatitude previous latitude angle.
21269 * @param oldLongitude previous longitude angle.
21270 * @param oldHeight previous height.
21271 * @param oldC previous body-to-NED coordinate transformation.
21272 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
21273 * resolved along NED-frame axes and expressed in meters per second (m/s).
21274 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
21275 * resolved along NED-frame axes and expressed in meters per second (m/s).
21276 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
21277 * resolved along NED-frame axes and expressed in meters per second (m/s).
21278 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
21279 * resolved along body-frame axes, averaged over time interval.
21280 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
21281 * resolved along body-frame axes, averaged over time interval.
21282 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
21283 * resolved along body-frame axes, averaged over time interval.
21284 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
21285 * resolved along body-frame axes, averaged over time interval.
21286 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
21287 * resolved along body-frame axes, averaged over time interval.
21288 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
21289 * resolved along body-frame axes, averaged over time interval.
21290 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
21291 * @return estimated NED frame containing new body position, velocity and coordinate
21292 * transformation matrix.
21293 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
21294 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
21295 * body-to-NED-frame coordinate transformation matrix are
21296 * invalid.
21297 */
21298 public static NEDFrame navigateNEDAndReturnNew(
21299 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
21300 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
21301 final Acceleration fx, final Acceleration fy, final Acceleration fz,
21302 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
21303 final double accuracyThreshold) throws InertialNavigatorException,
21304 InvalidSourceAndDestinationFrameTypeException {
21305 final var result = new NEDFrame();
21306 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
21307 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
21308 return result;
21309 }
21310
21311 /**
21312 * Runs precision local-navigation-frame inertial navigation equations.
21313 * NOTE: only the attitude update and specific force frame transformation
21314 * phases are precise.
21315 *
21316 * @param timeInterval time interval between epochs.
21317 * @param oldLatitude previous latitude angle.
21318 * @param oldLongitude previous longitude angle.
21319 * @param oldHeight previous height.
21320 * @param oldC previous body-to-NED coordinate transformation.
21321 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
21322 * resolved along NED-frame axes and expressed in meters per second (m/s).
21323 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
21324 * resolved along NED-frame axes and expressed in meters per second (m/s).
21325 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
21326 * resolved along NED-frame axes and expressed in meters per second (m/s).
21327 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
21328 * resolved along body-frame axes, averaged over time interval.
21329 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
21330 * resolved along body-frame axes, averaged over time interval.
21331 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
21332 * resolved along body-frame axes, averaged over time interval.
21333 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
21334 * resolved along body-frame axes, averaged over time interval.
21335 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
21336 * resolved along body-frame axes, averaged over time interval.
21337 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
21338 * resolved along body-frame axes, averaged over time interval.
21339 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
21340 * @return estimated NED frame containing new body position, velocity and coordinate
21341 * transformation matrix.
21342 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
21343 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
21344 * body-to-NED-frame coordinate transformation matrix are
21345 * invalid.
21346 */
21347 public static NEDFrame navigateNEDAndReturnNew(
21348 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
21349 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
21350 final Acceleration fx, final Acceleration fy, final Acceleration fz,
21351 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
21352 final double accuracyThreshold) throws InertialNavigatorException,
21353 InvalidSourceAndDestinationFrameTypeException {
21354 final var result = new NEDFrame();
21355 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
21356 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
21357 return result;
21358 }
21359
21360 /**
21361 * Runs precision local-navigation-frame inertial navigation equations.
21362 * NOTE: only the attitude update and specific force frame transformation
21363 * phases are precise.
21364 *
21365 * @param timeInterval time interval between epochs expressed in seconds (s).
21366 * @param oldPosition previous curvilinear position expressed in terms of latitude,
21367 * longitude and height.
21368 * @param oldC previous body-to-NED coordinate transformation.
21369 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
21370 * resolved along NED-frame axes and expressed in meters per second (m/s).
21371 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
21372 * resolved along NED-frame axes and expressed in meters per second (m/s).
21373 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
21374 * resolved along NED-frame axes and expressed in meters per second (m/s).
21375 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
21376 * resolved along body-frame axes, averaged over time interval.
21377 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
21378 * resolved along body-frame axes, averaged over time interval.
21379 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
21380 * resolved along body-frame axes, averaged over time interval.
21381 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
21382 * resolved along body-frame axes, averaged over time interval.
21383 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
21384 * resolved along body-frame axes, averaged over time interval.
21385 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
21386 * resolved along body-frame axes, averaged over time interval.
21387 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
21388 * @return estimated NED frame containing new body position, velocity and coordinate
21389 * transformation matrix.
21390 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
21391 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
21392 * body-to-NED-frame coordinate transformation matrix are
21393 * invalid.
21394 */
21395 public static NEDFrame navigateNEDAndReturnNew(
21396 final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
21397 final double oldVn, final double oldVe, final double oldVd,
21398 final Acceleration fx, final Acceleration fy, final Acceleration fz,
21399 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
21400 final double accuracyThreshold) throws InertialNavigatorException,
21401 InvalidSourceAndDestinationFrameTypeException {
21402 final var result = new NEDFrame();
21403 navigateNED(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
21404 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
21405 return result;
21406 }
21407
21408 /**
21409 * Runs precision local-navigation-frame inertial navigation equations.
21410 * NOTE: only the attitude update and specific force frame transformation
21411 * phases are precise.
21412 *
21413 * @param timeInterval time interval between epochs expressed in seconds (s).
21414 * @param oldPosition previous curvilinear position expressed in terms of latitude,
21415 * longitude and height.
21416 * @param oldC previous body-to-NED coordinate transformation.
21417 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
21418 * resolved along NED-frame axes and expressed in meters per second (m/s).
21419 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
21420 * resolved along NED-frame axes and expressed in meters per second (m/s).
21421 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
21422 * resolved along NED-frame axes and expressed in meters per second (m/s).
21423 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
21424 * resolved along body-frame axes, averaged over time interval.
21425 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
21426 * resolved along body-frame axes, averaged over time interval.
21427 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
21428 * resolved along body-frame axes, averaged over time interval.
21429 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
21430 * resolved along body-frame axes, averaged over time interval.
21431 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
21432 * resolved along body-frame axes, averaged over time interval.
21433 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
21434 * resolved along body-frame axes, averaged over time interval.
21435 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
21436 * @return estimated NED frame containing new body position, velocity and coordinate
21437 * transformation matrix.
21438 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
21439 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
21440 * body-to-NED-frame coordinate transformation matrix are
21441 * invalid.
21442 */
21443 public static NEDFrame navigateNEDAndReturnNew(
21444 final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
21445 final double oldVn, final double oldVe, final double oldVd,
21446 final Acceleration fx, final Acceleration fy, final Acceleration fz,
21447 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
21448 final double accuracyThreshold) throws InertialNavigatorException,
21449 InvalidSourceAndDestinationFrameTypeException {
21450 final var result = new NEDFrame();
21451 navigateNED(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
21452 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
21453 return result;
21454 }
21455
21456 /**
21457 * Runs precision local-navigation-frame inertial navigation equations.
21458 * NOTE: only the attitude update and specific force frame transformation
21459 * phases are precise.
21460 *
21461 * @param timeInterval time interval between epochs expressed in seconds (s).
21462 * @param oldLatitude previous latitude expressed in radians (rad).
21463 * @param oldLongitude previous longitude expressed in radians (rad).
21464 * @param oldHeight previous height expressed in meters (m).
21465 * @param oldC previous body-to-NED coordinate transformation.
21466 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
21467 * resolved along NED-frame axes and expressed in meters per second (m/s).
21468 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
21469 * resolved along NED-frame axes and expressed in meters per second (m/s).
21470 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
21471 * resolved along NED-frame axes and expressed in meters per second (m/s).
21472 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
21473 * resolved along body-frame axes, averaged over time interval.
21474 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
21475 * resolved along body-frame axes, averaged over time interval.
21476 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
21477 * resolved along body-frame axes, averaged over time interval.
21478 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
21479 * resolved along body-frame axes, averaged over time interval.
21480 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
21481 * resolved along body-frame axes, averaged over time interval.
21482 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
21483 * resolved along body-frame axes, averaged over time interval.
21484 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
21485 * @return estimated NED frame containing new body position, velocity and coordinate
21486 * transformation matrix.
21487 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
21488 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
21489 * body-to-NED-frame coordinate transformation matrix are
21490 * invalid.
21491 */
21492 public static NEDFrame navigateNEDAndReturnNew(
21493 final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
21494 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
21495 final Acceleration fx, final Acceleration fy, final Acceleration fz,
21496 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
21497 final double accuracyThreshold) throws InertialNavigatorException,
21498 InvalidSourceAndDestinationFrameTypeException {
21499 final var result = new NEDFrame();
21500 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
21501 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
21502 return result;
21503 }
21504
21505 /**
21506 * Runs precision local-navigation-frame inertial navigation equations.
21507 * NOTE: only the attitude update and specific force frame transformation
21508 * phases are precise.
21509 *
21510 * @param timeInterval time interval between epochs.
21511 * @param oldLatitude previous latitude expressed in radians (rad).
21512 * @param oldLongitude previous longitude expressed in radians (rad).
21513 * @param oldHeight previous height expressed in meters (m).
21514 * @param oldC previous body-to-NED coordinate transformation.
21515 * @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
21516 * resolved along NED-frame axes and expressed in meters per second (m/s).
21517 * @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
21518 * resolved along NED-frame axes and expressed in meters per second (m/s).
21519 * @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
21520 * resolved along NED-frame axes and expressed in meters per second (m/s).
21521 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
21522 * resolved along body-frame axes, averaged over time interval.
21523 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
21524 * resolved along body-frame axes, averaged over time interval.
21525 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
21526 * resolved along body-frame axes, averaged over time interval.
21527 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
21528 * resolved along body-frame axes, averaged over time interval.
21529 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
21530 * resolved along body-frame axes, averaged over time interval.
21531 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
21532 * resolved along body-frame axes, averaged over time interval.
21533 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
21534 * @return estimated NED frame containing new body position, velocity and coordinate
21535 * transformation matrix.
21536 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
21537 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
21538 * body-to-NED-frame coordinate transformation matrix are
21539 * invalid.
21540 */
21541 public static NEDFrame navigateNEDAndReturnNew(
21542 final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
21543 final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
21544 final Acceleration fx, final Acceleration fy, final Acceleration fz,
21545 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
21546 final double accuracyThreshold) throws InertialNavigatorException,
21547 InvalidSourceAndDestinationFrameTypeException {
21548 final var result = new NEDFrame();
21549 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
21550 angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
21551 return result;
21552 }
21553
21554 /**
21555 * Runs precision local-navigation-frame inertial navigation equations.
21556 * NOTE: only the attitude update and specific force frame transformation
21557 * phases are precise.
21558 *
21559 * @param timeInterval time interval between epochs expressed in seconds (s).
21560 * @param oldLatitude previous latitude angle.
21561 * @param oldLongitude previous longitude angle.
21562 * @param oldHeight previous height.
21563 * @param oldC previous body-to-NED coordinate transformation.
21564 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
21565 * resolved along NED-frame axes.
21566 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
21567 * resolved along NED-frame axes.
21568 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
21569 * resolved along NED-frame axes.
21570 * @param kinematics body kinematics containing specific forces and angular rates applied to
21571 * the body.
21572 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
21573 * @return estimated NED frame containing new body position, velocity and coordinate
21574 * transformation matrix.
21575 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
21576 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
21577 * body-to-NED-frame coordinate transformation matrix are
21578 * invalid.
21579 */
21580 public static NEDFrame navigateNEDAndReturnNew(
21581 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
21582 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
21583 final BodyKinematics kinematics, final double accuracyThreshold) throws InertialNavigatorException,
21584 InvalidSourceAndDestinationFrameTypeException {
21585 final var result = new NEDFrame();
21586 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
21587 kinematics, accuracyThreshold, result);
21588 return result;
21589 }
21590
21591 /**
21592 * Runs precision local-navigation-frame inertial navigation equations.
21593 * NOTE: only the attitude update and specific force frame transformation
21594 * phases are precise.
21595 *
21596 * @param timeInterval time interval between epochs expressed in seconds (s).
21597 * @param oldLatitude previous latitude angle.
21598 * @param oldLongitude previous longitude angle.
21599 * @param oldHeight previous height.
21600 * @param oldC previous body-to-NED coordinate transformation.
21601 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
21602 * resolved along NED-frame axes.
21603 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
21604 * resolved along NED-frame axes.
21605 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
21606 * resolved along NED-frame axes.
21607 * @param kinematics body kinematics containing specific forces and angular rates applied to
21608 * the body.
21609 * @return estimated NED frame containing new body position, velocity and coordinate
21610 * transformation matrix.
21611 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
21612 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
21613 * body-to-NED-frame coordinate transformation matrix are
21614 * invalid.
21615 */
21616 public static NEDFrame navigateNEDAndReturnNew(
21617 final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
21618 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
21619 final BodyKinematics kinematics) throws InertialNavigatorException,
21620 InvalidSourceAndDestinationFrameTypeException {
21621 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
21622 oldSpeedN, oldSpeedE, oldSpeedD, kinematics, DEFAULT_ACCURACY_THRESHOLD);
21623 }
21624
21625 /**
21626 * Runs precision local-navigation-frame inertial navigation equations.
21627 * NOTE: only the attitude update and specific force frame transformation
21628 * phases are precise.
21629 *
21630 * @param timeInterval time interval between epochs.
21631 * @param oldLatitude previous latitude angle.
21632 * @param oldLongitude previous longitude angle.
21633 * @param oldHeight previous height.
21634 * @param oldC previous body-to-NED coordinate transformation.
21635 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
21636 * resolved along NED-frame axes.
21637 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
21638 * resolved along NED-frame axes.
21639 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
21640 * resolved along NED-frame axes.
21641 * @param kinematics body kinematics containing specific forces and angular rates applied to
21642 * the body.
21643 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
21644 * @return estimated NED frame containing new body position, velocity and coordinate
21645 * transformation matrix.
21646 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
21647 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
21648 * body-to-NED-frame coordinate transformation matrix are
21649 * invalid.
21650 */
21651 public static NEDFrame navigateNEDAndReturnNew(
21652 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
21653 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE,
21654 final Speed oldSpeedD, final BodyKinematics kinematics, final double accuracyThreshold)
21655 throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
21656 final var result = new NEDFrame();
21657 navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
21658 kinematics, accuracyThreshold, result);
21659 return result;
21660 }
21661
21662 /**
21663 * Runs precision local-navigation-frame inertial navigation equations.
21664 * NOTE: only the attitude update and specific force frame transformation
21665 * phases are precise.
21666 *
21667 * @param timeInterval time interval between epochs.
21668 * @param oldLatitude previous latitude angle.
21669 * @param oldLongitude previous longitude angle.
21670 * @param oldHeight previous height.
21671 * @param oldC previous body-to-NED coordinate transformation.
21672 * @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
21673 * resolved along NED-frame axes.
21674 * @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
21675 * resolved along NED-frame axes.
21676 * @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
21677 * resolved along NED-frame axes.
21678 * @param kinematics body kinematics containing specific forces and angular rates applied to
21679 * the body.
21680 * @return estimated NED frame containing new body position, velocity and coordinate
21681 * transformation matrix.
21682 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
21683 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
21684 * body-to-NED-frame coordinate transformation matrix are
21685 * invalid.
21686 */
21687 public static NEDFrame navigateNEDAndReturnNew(
21688 final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
21689 final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
21690 final BodyKinematics kinematics) throws InertialNavigatorException,
21691 InvalidSourceAndDestinationFrameTypeException {
21692 return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
21693 oldSpeedN, oldSpeedE, oldSpeedD, kinematics, DEFAULT_ACCURACY_THRESHOLD);
21694 }
21695
21696 /**
21697 * Runs precision local-navigation-frame inertial navigation equations.
21698 * NOTE: only the attitude update and specific force frame transformation
21699 * phases are precise.
21700 *
21701 * @param timeInterval time interval between epochs expressed in seconds (s).
21702 * @param oldFrame previous NED frame containing body position, velocity and
21703 * coordinate transformation matrix.
21704 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
21705 * resolved along body-frame axes, averaged over time interval and
21706 * expressed in meters per squared second (m/s^2).
21707 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
21708 * resolved along body-frame axes, averaged over time interval and
21709 * expressed in meters per squared second (m/s^2).
21710 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
21711 * resolved along body-frame axes, averaged over time interval and
21712 * expressed in meters per squared second (m/s^2).
21713 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
21714 * resolved along body-frame axes, averaged over time interval and
21715 * expressed in radians per second (rad/s).
21716 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
21717 * resolved along body-frame axes, averaged over time interval and
21718 * expressed in radians per second (rad/s).
21719 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
21720 * resolved along body-frame axes, averaged over time interval and
21721 * expressed in radians per second (rad/s).
21722 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
21723 * @return estimated NED frame containing new body position, velocity and coordinate
21724 * transformation matrix.
21725 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
21726 */
21727 public static NEDFrame navigateNEDAndReturnNew(
21728 final double timeInterval, final NEDFrame oldFrame, final double fx, final double fy, final double fz,
21729 final double angularRateX, final double angularRateY, final double angularRateZ,
21730 final double accuracyThreshold) throws InertialNavigatorException {
21731 final var result = new NEDFrame();
21732 navigateNED(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold,
21733 result);
21734 return result;
21735 }
21736
21737 /**
21738 * Runs precision local-navigation-frame inertial navigation equations.
21739 * NOTE: only the attitude update and specific force frame transformation
21740 * phases are precise.
21741 *
21742 * @param timeInterval time interval between epochs expressed in seconds (s).
21743 * @param oldFrame previous NED frame containing body position, velocity and
21744 * coordinate transformation matrix.
21745 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
21746 * resolved along body-frame axes, averaged over time interval and
21747 * expressed in meters per squared second (m/s^2).
21748 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
21749 * resolved along body-frame axes, averaged over time interval and
21750 * expressed in meters per squared second (m/s^2).
21751 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
21752 * resolved along body-frame axes, averaged over time interval and
21753 * expressed in meters per squared second (m/s^2).
21754 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
21755 * resolved along body-frame axes, averaged over time interval and
21756 * expressed in radians per second (rad/s).
21757 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
21758 * resolved along body-frame axes, averaged over time interval and
21759 * expressed in radians per second (rad/s).
21760 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
21761 * resolved along body-frame axes, averaged over time interval and
21762 * expressed in radians per second (rad/s).
21763 * @return estimated NED frame containing new body position, velocity and coordinate
21764 * transformation matrix.
21765 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
21766 */
21767 public static NEDFrame navigateNEDAndReturnNew(
21768 final double timeInterval, final NEDFrame oldFrame, final double fx, final double fy, final double fz,
21769 final double angularRateX, final double angularRateY, final double angularRateZ)
21770 throws InertialNavigatorException {
21771 return navigateNEDAndReturnNew(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
21772 DEFAULT_ACCURACY_THRESHOLD);
21773 }
21774
21775 /**
21776 * Runs precision local-navigation-frame inertial navigation equations.
21777 * NOTE: only the attitude update and specific force frame transformation
21778 * phases are precise.
21779 *
21780 * @param timeInterval time interval between epochs.
21781 * @param oldFrame previous NED frame containing body position, velocity and
21782 * coordinate transformation matrix.
21783 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
21784 * resolved along body-frame axes, averaged over time interval and
21785 * expressed in meters per squared second (m/s^2).
21786 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
21787 * resolved along body-frame axes, averaged over time interval and
21788 * expressed in meters per squared second (m/s^2).
21789 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
21790 * resolved along body-frame axes, averaged over time interval and
21791 * expressed in meters per squared second (m/s^2).
21792 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
21793 * resolved along body-frame axes, averaged over time interval and
21794 * expressed in radians per second (rad/s).
21795 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
21796 * resolved along body-frame axes, averaged over time interval and
21797 * expressed in radians per second (rad/s).
21798 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
21799 * resolved along body-frame axes, averaged over time interval and
21800 * expressed in radians per second (rad/s).
21801 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
21802 * @return estimated NED frame containing new body position, velocity and coordinate
21803 * transformation matrix.
21804 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
21805 */
21806 public static NEDFrame navigateNEDAndReturnNew(
21807 final Time timeInterval, final NEDFrame oldFrame, final double fx, final double fy, final double fz,
21808 final double angularRateX, final double angularRateY, final double angularRateZ,
21809 final double accuracyThreshold) throws InertialNavigatorException {
21810 final var result = new NEDFrame();
21811 navigateNED(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold,
21812 result);
21813 return result;
21814 }
21815
21816 /**
21817 * Runs precision local-navigation-frame inertial navigation equations.
21818 * NOTE: only the attitude update and specific force frame transformation
21819 * phases are precise.
21820 *
21821 * @param timeInterval time interval between epochs.
21822 * @param oldFrame previous NED frame containing body position, velocity and
21823 * coordinate transformation matrix.
21824 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
21825 * resolved along body-frame axes, averaged over time interval and
21826 * expressed in meters per squared second (m/s^2).
21827 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
21828 * resolved along body-frame axes, averaged over time interval and
21829 * expressed in meters per squared second (m/s^2).
21830 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
21831 * resolved along body-frame axes, averaged over time interval and
21832 * expressed in meters per squared second (m/s^2).
21833 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
21834 * resolved along body-frame axes, averaged over time interval and
21835 * expressed in radians per second (rad/s).
21836 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
21837 * resolved along body-frame axes, averaged over time interval and
21838 * expressed in radians per second (rad/s).
21839 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
21840 * resolved along body-frame axes, averaged over time interval and
21841 * expressed in radians per second (rad/s).
21842 * @return estimated NED frame containing new body position, velocity and coordinate
21843 * transformation matrix.
21844 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
21845 */
21846 public static NEDFrame navigateNEDAndReturnNew(
21847 final Time timeInterval, final NEDFrame oldFrame, final double fx, final double fy, final double fz,
21848 final double angularRateX, final double angularRateY, final double angularRateZ)
21849 throws InertialNavigatorException {
21850 return navigateNEDAndReturnNew(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
21851 DEFAULT_ACCURACY_THRESHOLD);
21852 }
21853
21854 /**
21855 * Runs precision local-navigation-frame inertial navigation equations.
21856 * NOTE: only the attitude update and specific force frame transformation
21857 * phases are precise.
21858 *
21859 * @param timeInterval time interval between epochs expressed in seconds (s).
21860 * @param oldFrame previous NED frame containing body position, velocity and
21861 * coordinate transformation matrix.
21862 * @param kinematics body kinematics containing specific forces and angular rates applied to
21863 * the body.
21864 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
21865 * @return estimated NED frame containing new body position, velocity and coordinate
21866 * transformation matrix.
21867 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
21868 */
21869 public static NEDFrame navigateNEDAndReturnNew(
21870 final double timeInterval, final NEDFrame oldFrame, final BodyKinematics kinematics,
21871 final double accuracyThreshold) throws InertialNavigatorException {
21872 final var result = new NEDFrame();
21873 navigateNED(timeInterval, oldFrame, kinematics, accuracyThreshold, result);
21874 return result;
21875 }
21876
21877 /**
21878 * Runs precision local-navigation-frame inertial navigation equations.
21879 * NOTE: only the attitude update and specific force frame transformation
21880 * phases are precise.
21881 *
21882 * @param timeInterval time interval between epochs expressed in seconds (s).
21883 * @param oldFrame previous NED frame containing body position, velocity and
21884 * coordinate transformation matrix.
21885 * @param kinematics body kinematics containing specific forces and angular rates applied to
21886 * the body.
21887 * @return estimated NED frame containing new body position, velocity and coordinate
21888 * transformation matrix.
21889 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
21890 */
21891 public static NEDFrame navigateNEDAndReturnNew(
21892 final double timeInterval, final NEDFrame oldFrame, final BodyKinematics kinematics)
21893 throws InertialNavigatorException {
21894 return navigateNEDAndReturnNew(timeInterval, oldFrame, kinematics, DEFAULT_ACCURACY_THRESHOLD);
21895 }
21896
21897 /**
21898 * Runs precision local-navigation-frame inertial navigation equations.
21899 * NOTE: only the attitude update and specific force frame transformation
21900 * phases are precise.
21901 *
21902 * @param timeInterval time interval between epochs.
21903 * @param oldFrame previous NED frame containing body position, velocity and
21904 * coordinate transformation matrix.
21905 * @param kinematics body kinematics containing specific forces and angular rates applied to
21906 * the body.
21907 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
21908 * @return estimated NED frame containing new body position, velocity and coordinate
21909 * transformation matrix.
21910 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
21911 */
21912 public static NEDFrame navigateNEDAndReturnNew(
21913 final Time timeInterval, final NEDFrame oldFrame, final BodyKinematics kinematics,
21914 final double accuracyThreshold) throws InertialNavigatorException {
21915 final var result = new NEDFrame();
21916 navigateNED(timeInterval, oldFrame, kinematics, accuracyThreshold, result);
21917 return result;
21918 }
21919
21920 /**
21921 * Runs precision local-navigation-frame inertial navigation equations.
21922 * NOTE: only the attitude update and specific force frame transformation
21923 * phases are precise.
21924 *
21925 * @param timeInterval time interval between epochs.
21926 * @param oldFrame previous NED frame containing body position, velocity and
21927 * coordinate transformation matrix.
21928 * @param kinematics body kinematics containing specific forces and angular rates applied to
21929 * the body.
21930 * @return estimated NED frame containing new body position, velocity and coordinate
21931 * transformation matrix.
21932 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
21933 */
21934 public static NEDFrame navigateNEDAndReturnNew(
21935 final Time timeInterval, final NEDFrame oldFrame, final BodyKinematics kinematics)
21936 throws InertialNavigatorException {
21937 return navigateNEDAndReturnNew(timeInterval, oldFrame, kinematics, DEFAULT_ACCURACY_THRESHOLD);
21938 }
21939
21940 /**
21941 * Runs precision local-navigation-frame inertial navigation equations.
21942 * NOTE: only the attitude update and specific force frame transformation
21943 * phases are precise.
21944 *
21945 * @param timeInterval time interval between epochs expressed in seconds (s).
21946 * @param oldFrame previous NED frame containing body position, velocity and
21947 * coordinate transformation matrix.
21948 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
21949 * resolved along body-frame axes, averaged over time interval.
21950 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
21951 * resolved along body-frame axes, averaged over time interval.
21952 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
21953 * resolved along body-frame axes, averaged over time interval.
21954 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
21955 * resolved along body-frame axes, averaged over time interval and
21956 * expressed in radians per second (rad/s).
21957 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
21958 * resolved along body-frame axes, averaged over time interval and
21959 * expressed in radians per second (rad/s).
21960 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
21961 * resolved along body-frame axes, averaged over time interval and
21962 * expressed in radians per second (rad/s).
21963 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
21964 * @return estimated NED frame containing new body position, velocity and coordinate
21965 * transformation matrix.
21966 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
21967 */
21968 public static NEDFrame navigateNEDAndReturnNew(
21969 final double timeInterval, final NEDFrame oldFrame,
21970 final Acceleration fx, final Acceleration fy, final Acceleration fz,
21971 final double angularRateX, final double angularRateY, final double angularRateZ,
21972 final double accuracyThreshold) throws InertialNavigatorException {
21973 final var result = new NEDFrame();
21974 navigateNED(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold,
21975 result);
21976 return result;
21977 }
21978
21979 /**
21980 * Runs precision local-navigation-frame inertial navigation equations.
21981 * NOTE: only the attitude update and specific force frame transformation
21982 * phases are precise.
21983 *
21984 * @param timeInterval time interval between epochs expressed in seconds (s).
21985 * @param oldFrame previous NED frame containing body position, velocity and
21986 * coordinate transformation matrix.
21987 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
21988 * resolved along body-frame axes, averaged over time interval.
21989 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
21990 * resolved along body-frame axes, averaged over time interval.
21991 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
21992 * resolved along body-frame axes, averaged over time interval.
21993 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
21994 * resolved along body-frame axes, averaged over time interval and
21995 * expressed in radians per second (rad/s).
21996 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
21997 * resolved along body-frame axes, averaged over time interval and
21998 * expressed in radians per second (rad/s).
21999 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
22000 * resolved along body-frame axes, averaged over time interval and
22001 * expressed in radians per second (rad/s).
22002 * @return estimated NED frame containing new body position, velocity and coordinate
22003 * transformation matrix.
22004 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
22005 */
22006 public static NEDFrame navigateNEDAndReturnNew(
22007 final double timeInterval, final NEDFrame oldFrame,
22008 final Acceleration fx, final Acceleration fy, final Acceleration fz,
22009 final double angularRateX, final double angularRateY, final double angularRateZ)
22010 throws InertialNavigatorException {
22011 return navigateNEDAndReturnNew(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
22012 DEFAULT_ACCURACY_THRESHOLD);
22013 }
22014
22015 /**
22016 * Runs precision local-navigation-frame inertial navigation equations.
22017 * NOTE: only the attitude update and specific force frame transformation
22018 * phases are precise.
22019 *
22020 * @param timeInterval time interval between epochs.
22021 * @param oldFrame previous NED frame containing body position, velocity and
22022 * coordinate transformation matrix.
22023 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
22024 * resolved along body-frame axes, averaged over time interval.
22025 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
22026 * resolved along body-frame axes, averaged over time interval.
22027 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
22028 * resolved along body-frame axes, averaged over time interval.
22029 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
22030 * resolved along body-frame axes, averaged over time interval and
22031 * expressed in radians per second (rad/s).
22032 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
22033 * resolved along body-frame axes, averaged over time interval and
22034 * expressed in radians per second (rad/s).
22035 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
22036 * resolved along body-frame axes, averaged over time interval and
22037 * expressed in radians per second (rad/s).
22038 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
22039 * @return estimated NED frame containing new body position, velocity and coordinate
22040 * transformation matrix.
22041 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
22042 */
22043 public static NEDFrame navigateNEDAndReturnNew(
22044 final Time timeInterval, final NEDFrame oldFrame,
22045 final Acceleration fx, final Acceleration fy, final Acceleration fz,
22046 final double angularRateX, final double angularRateY, final double angularRateZ,
22047 final double accuracyThreshold) throws InertialNavigatorException {
22048 final var result = new NEDFrame();
22049 navigateNED(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold,
22050 result);
22051 return result;
22052 }
22053
22054 /**
22055 * Runs precision local-navigation-frame inertial navigation equations.
22056 * NOTE: only the attitude update and specific force frame transformation
22057 * phases are precise.
22058 *
22059 * @param timeInterval time interval between epochs.
22060 * @param oldFrame previous NED frame containing body position, velocity and
22061 * coordinate transformation matrix.
22062 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
22063 * resolved along body-frame axes, averaged over time interval.
22064 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
22065 * resolved along body-frame axes, averaged over time interval.
22066 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
22067 * resolved along body-frame axes, averaged over time interval.
22068 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
22069 * resolved along body-frame axes, averaged over time interval and
22070 * expressed in radians per second (rad/s).
22071 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
22072 * resolved along body-frame axes, averaged over time interval and
22073 * expressed in radians per second (rad/s).
22074 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
22075 * resolved along body-frame axes, averaged over time interval and
22076 * expressed in radians per second (rad/s).
22077 * @return estimated NED frame containing new body position, velocity and coordinate
22078 * transformation matrix.
22079 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
22080 */
22081 public static NEDFrame navigateNEDAndReturnNew(
22082 final Time timeInterval, final NEDFrame oldFrame,
22083 final Acceleration fx, final Acceleration fy, final Acceleration fz,
22084 final double angularRateX, final double angularRateY, final double angularRateZ)
22085 throws InertialNavigatorException {
22086 return navigateNEDAndReturnNew(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
22087 DEFAULT_ACCURACY_THRESHOLD);
22088 }
22089
22090 /**
22091 * Runs precision local-navigation-frame inertial navigation equations.
22092 * NOTE: only the attitude update and specific force frame transformation
22093 * phases are precise.
22094 *
22095 * @param timeInterval time interval between epochs expressed in seconds (s).
22096 * @param oldFrame previous NED frame containing body position, velocity and
22097 * coordinate transformation matrix.
22098 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
22099 * resolved along body-frame axes, averaged over time interval and
22100 * expressed in meters per squared second (m/s^2).
22101 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
22102 * resolved along body-frame axes, averaged over time interval and
22103 * expressed in meters per squared second (m/s^2).
22104 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
22105 * resolved along body-frame axes, averaged over time interval and
22106 * expressed in meters per squared second (m/s^2).
22107 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
22108 * resolved along body-frame axes, averaged over time interval.
22109 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
22110 * resolved along body-frame axes, averaged over time interval.
22111 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
22112 * resolved along body-frame axes, averaged over time interval.
22113 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
22114 * @return estimated NED frame containing new body position, velocity and coordinate
22115 * transformation matrix.
22116 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
22117 */
22118 public static NEDFrame navigateNEDAndReturnNew(
22119 final double timeInterval, final NEDFrame oldFrame,
22120 final double fx, final double fy, final double fz,
22121 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
22122 final double accuracyThreshold) throws InertialNavigatorException {
22123 final var result = new NEDFrame();
22124 navigateNED(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold,
22125 result);
22126 return result;
22127 }
22128
22129 /**
22130 * Runs precision local-navigation-frame inertial navigation equations.
22131 * NOTE: only the attitude update and specific force frame transformation
22132 * phases are precise.
22133 *
22134 * @param timeInterval time interval between epochs expressed in seconds (s).
22135 * @param oldFrame previous NED frame containing body position, velocity and
22136 * coordinate transformation matrix.
22137 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
22138 * resolved along body-frame axes, averaged over time interval and
22139 * expressed in meters per squared second (m/s^2).
22140 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
22141 * resolved along body-frame axes, averaged over time interval and
22142 * expressed in meters per squared second (m/s^2).
22143 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
22144 * resolved along body-frame axes, averaged over time interval and
22145 * expressed in meters per squared second (m/s^2).
22146 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
22147 * resolved along body-frame axes, averaged over time interval.
22148 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
22149 * resolved along body-frame axes, averaged over time interval.
22150 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
22151 * resolved along body-frame axes, averaged over time interval.
22152 * @return estimated NED frame containing new body position, velocity and coordinate
22153 * transformation matrix.
22154 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
22155 */
22156 public static NEDFrame navigateNEDAndReturnNew(
22157 final double timeInterval, final NEDFrame oldFrame, final double fx, final double fy, final double fz,
22158 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
22159 throws InertialNavigatorException {
22160 return navigateNEDAndReturnNew(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
22161 DEFAULT_ACCURACY_THRESHOLD);
22162 }
22163
22164 /**
22165 * Runs precision local-navigation-frame inertial navigation equations.
22166 * NOTE: only the attitude update and specific force frame transformation
22167 * phases are precise.
22168 *
22169 * @param timeInterval time interval between epochs.
22170 * @param oldFrame previous NED frame containing body position, velocity and
22171 * coordinate transformation matrix.
22172 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
22173 * resolved along body-frame axes, averaged over time interval and
22174 * expressed in meters per squared second (m/s^2).
22175 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
22176 * resolved along body-frame axes, averaged over time interval and
22177 * expressed in meters per squared second (m/s^2).
22178 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
22179 * resolved along body-frame axes, averaged over time interval and
22180 * expressed in meters per squared second (m/s^2).
22181 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
22182 * resolved along body-frame axes, averaged over time interval.
22183 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
22184 * resolved along body-frame axes, averaged over time interval.
22185 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
22186 * resolved along body-frame axes, averaged over time interval.
22187 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
22188 * @return estimated NED frame containing new body position, velocity and coordinate
22189 * transformation matrix.
22190 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
22191 */
22192 public static NEDFrame navigateNEDAndReturnNew(
22193 final Time timeInterval, final NEDFrame oldFrame, final double fx, final double fy, final double fz,
22194 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
22195 final double accuracyThreshold) throws InertialNavigatorException {
22196 final var result = new NEDFrame();
22197 navigateNED(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold,
22198 result);
22199 return result;
22200 }
22201
22202 /**
22203 * Runs precision local-navigation-frame inertial navigation equations.
22204 * NOTE: only the attitude update and specific force frame transformation
22205 * phases are precise.
22206 *
22207 * @param timeInterval time interval between epochs.
22208 * @param oldFrame previous NED frame containing body position, velocity and
22209 * coordinate transformation matrix.
22210 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
22211 * resolved along body-frame axes, averaged over time interval and
22212 * expressed in meters per squared second (m/s^2).
22213 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
22214 * resolved along body-frame axes, averaged over time interval and
22215 * expressed in meters per squared second (m/s^2).
22216 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
22217 * resolved along body-frame axes, averaged over time interval and
22218 * expressed in meters per squared second (m/s^2).
22219 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
22220 * resolved along body-frame axes, averaged over time interval.
22221 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
22222 * resolved along body-frame axes, averaged over time interval.
22223 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
22224 * resolved along body-frame axes, averaged over time interval.
22225 * @return estimated NED frame containing new body position, velocity and coordinate
22226 * transformation matrix.
22227 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
22228 */
22229 public static NEDFrame navigateNEDAndReturnNew(
22230 final Time timeInterval, final NEDFrame oldFrame, final double fx, final double fy, final double fz,
22231 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
22232 throws InertialNavigatorException {
22233 return navigateNEDAndReturnNew(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
22234 DEFAULT_ACCURACY_THRESHOLD);
22235 }
22236
22237 /**
22238 * Runs precision local-navigation-frame inertial navigation equations.
22239 * NOTE: only the attitude update and specific force frame transformation
22240 * phases are precise.
22241 *
22242 * @param timeInterval time interval between epochs expressed in seconds (s).
22243 * @param oldFrame previous NED frame containing body position, velocity and
22244 * coordinate transformation matrix.
22245 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
22246 * resolved along body-frame axes, averaged over time interval and
22247 * expressed in meters per squared second (m/s^2).
22248 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
22249 * resolved along body-frame axes, averaged over time interval and
22250 * expressed in meters per squared second (m/s^2).
22251 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
22252 * resolved along body-frame axes, averaged over time interval and
22253 * expressed in meters per squared second (m/s^2).
22254 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
22255 * resolved along body-frame axes, averaged over time interval.
22256 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
22257 * resolved along body-frame axes, averaged over time interval.
22258 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
22259 * resolved along body-frame axes, averaged over time interval.
22260 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
22261 * @return estimated NED frame containing new body position, velocity and coordinate
22262 * transformation matrix.
22263 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
22264 */
22265 public static NEDFrame navigateNEDAndReturnNew(
22266 final double timeInterval, final NEDFrame oldFrame,
22267 final Acceleration fx, final Acceleration fy, final Acceleration fz,
22268 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
22269 final double accuracyThreshold) throws InertialNavigatorException {
22270 final var result = new NEDFrame();
22271 navigateNED(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold,
22272 result);
22273 return result;
22274 }
22275
22276 /**
22277 * Runs precision local-navigation-frame inertial navigation equations.
22278 * NOTE: only the attitude update and specific force frame transformation
22279 * phases are precise.
22280 *
22281 * @param timeInterval time interval between epochs expressed in seconds (s).
22282 * @param oldFrame previous NED frame containing body position, velocity and
22283 * coordinate transformation matrix.
22284 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
22285 * resolved along body-frame axes, averaged over time interval and
22286 * expressed in meters per squared second (m/s^2).
22287 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
22288 * resolved along body-frame axes, averaged over time interval and
22289 * expressed in meters per squared second (m/s^2).
22290 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
22291 * resolved along body-frame axes, averaged over time interval and
22292 * expressed in meters per squared second (m/s^2).
22293 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
22294 * resolved along body-frame axes, averaged over time interval.
22295 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
22296 * resolved along body-frame axes, averaged over time interval.
22297 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
22298 * resolved along body-frame axes, averaged over time interval.
22299 * @return estimated NED frame containing new body position, velocity and coordinate
22300 * transformation matrix.
22301 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
22302 */
22303 public static NEDFrame navigateNEDAndReturnNew(
22304 final double timeInterval, final NEDFrame oldFrame,
22305 final Acceleration fx, final Acceleration fy, final Acceleration fz,
22306 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
22307 throws InertialNavigatorException {
22308 return navigateNEDAndReturnNew(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
22309 DEFAULT_ACCURACY_THRESHOLD);
22310 }
22311
22312 /**
22313 * Runs precision local-navigation-frame inertial navigation equations.
22314 * NOTE: only the attitude update and specific force frame transformation
22315 * phases are precise.
22316 *
22317 * @param timeInterval time interval between epochs expressed in seconds (s).
22318 * @param oldFrame previous NED frame containing body position, velocity and
22319 * coordinate transformation matrix.
22320 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
22321 * resolved along body-frame axes, averaged over time interval and
22322 * expressed in meters per squared second (m/s^2).
22323 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
22324 * resolved along body-frame axes, averaged over time interval and
22325 * expressed in meters per squared second (m/s^2).
22326 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
22327 * resolved along body-frame axes, averaged over time interval and
22328 * expressed in meters per squared second (m/s^2).
22329 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
22330 * resolved along body-frame axes, averaged over time interval.
22331 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
22332 * resolved along body-frame axes, averaged over time interval.
22333 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
22334 * resolved along body-frame axes, averaged over time interval.
22335 * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
22336 * @return estimated NED frame containing new body position, velocity and coordinate
22337 * transformation matrix.
22338 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
22339 */
22340 public static NEDFrame navigateNEDAndReturnNew(
22341 final Time timeInterval, final NEDFrame oldFrame,
22342 final Acceleration fx, final Acceleration fy, final Acceleration fz,
22343 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
22344 final double accuracyThreshold) throws InertialNavigatorException {
22345 final var result = new NEDFrame();
22346 navigateNED(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold,
22347 result);
22348 return result;
22349 }
22350
22351 /**
22352 * Runs precision local-navigation-frame inertial navigation equations.
22353 * NOTE: only the attitude update and specific force frame transformation
22354 * phases are precise.
22355 *
22356 * @param timeInterval time interval between epochs expressed in seconds (s).
22357 * @param oldFrame previous NED frame containing body position, velocity and
22358 * coordinate transformation matrix.
22359 * @param fx specific force x-coordinate of body frame with respect ECEF frame,
22360 * resolved along body-frame axes, averaged over time interval and
22361 * expressed in meters per squared second (m/s^2).
22362 * @param fy specific force y-coordinate of body frame with respect ECEF frame,
22363 * resolved along body-frame axes, averaged over time interval and
22364 * expressed in meters per squared second (m/s^2).
22365 * @param fz specific force z-coordinate of body frame with respect ECEF frame,
22366 * resolved along body-frame axes, averaged over time interval and
22367 * expressed in meters per squared second (m/s^2).
22368 * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
22369 * resolved along body-frame axes, averaged over time interval.
22370 * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
22371 * resolved along body-frame axes, averaged over time interval.
22372 * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
22373 * resolved along body-frame axes, averaged over time interval.
22374 * @return estimated NED frame containing new body position, velocity and coordinate
22375 * transformation matrix.
22376 * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
22377 */
22378 public static NEDFrame navigateNEDAndReturnNew(
22379 final Time timeInterval, final NEDFrame oldFrame,
22380 final Acceleration fx, final Acceleration fy, final Acceleration fz,
22381 final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
22382 throws InertialNavigatorException {
22383 return navigateNEDAndReturnNew(timeInterval, oldFrame, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
22384 DEFAULT_ACCURACY_THRESHOLD);
22385 }
22386
22387 /**
22388 * Checks whether provided coordinate transformation matrix is valid or not.
22389 * Only body to NED transformation matrices are considered to be valid.
22390 *
22391 * @param c coordinate transformation matrix to be checked.
22392 * @return true if provided value is valid, false otherwise.
22393 */
22394 public static boolean isValidBodyToNEDCoordinateTransformationMatrix(final CoordinateTransformation c) {
22395 return NEDFrame.isValidCoordinateTransformation(c);
22396 }
22397
22398 /**
22399 * Converts provided time instance into its corresponding value expressed in
22400 * seconds.
22401 *
22402 * @param time time instance to be converted.
22403 * @return converted value expressed in seconds.
22404 */
22405 private static double convertTimeToDouble(final Time time) {
22406 return TimeConverter.convert(time.getValue().doubleValue(), time.getUnit(), TimeUnit.SECOND);
22407 }
22408
22409 /**
22410 * Converts provided angle instance into its corresponding value expressed in
22411 * radians.
22412 *
22413 * @param angle angle instance to be converted.
22414 * @return converted value expressed in meters.
22415 */
22416 private static double convertAngleToDouble(final Angle angle) {
22417 return AngleConverter.convert(angle.getValue().doubleValue(), angle.getUnit(), AngleUnit.RADIANS);
22418 }
22419
22420 /**
22421 * Converts provided distance instance into its corresponding value expressed in
22422 * meters.
22423 *
22424 * @param distance distance instance to be converted.
22425 * @return converted value expressed in meters.
22426 */
22427 private static double convertDistanceToDouble(final Distance distance) {
22428 return DistanceConverter.convert(distance.getValue().doubleValue(), distance.getUnit(), DistanceUnit.METER);
22429 }
22430
22431 /**
22432 * Converts provided speed instance into its corresponding value expressed in
22433 * meters per second.
22434 *
22435 * @param speed speed instance to be converted.
22436 * @return converted value expressed in meters per second.
22437 */
22438 private static double convertSpeedToDouble(final Speed speed) {
22439 return SpeedConverter.convert(speed.getValue().doubleValue(), speed.getUnit(), SpeedUnit.METERS_PER_SECOND);
22440 }
22441
22442 /**
22443 * Converts provided acceleration instance into its corresponding value expressed
22444 * in meters per squared second.
22445 *
22446 * @param acceleration acceleration instance to be converted.
22447 * @return converted value expressed in meters per squared second.
22448 */
22449 private static double convertAccelerationToDouble(final Acceleration acceleration) {
22450 return AccelerationConverter.convert(acceleration.getValue().doubleValue(), acceleration.getUnit(),
22451 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22452 }
22453
22454 /**
22455 * Converts provided angular speed into its corresponding value expressed in
22456 * radians per second.
22457 *
22458 * @param angularSpeed angular speed instance to be converted.
22459 * @return converted value expressed in radians per second.
22460 */
22461 private static double convertAngularSpeedToDouble(final AngularSpeed angularSpeed) {
22462 return AngularSpeedConverter.convert(angularSpeed.getValue().doubleValue(), angularSpeed.getUnit(),
22463 AngularSpeedUnit.RADIANS_PER_SECOND);
22464 }
22465 }