1 /*
2 * Copyright (C) 2019 Alberto Irurueta Carro (alberto@irurueta.com)
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16 package com.irurueta.navigation.inertial.estimators;
17
18 import com.irurueta.algebra.AlgebraException;
19 import com.irurueta.algebra.Matrix;
20 import com.irurueta.algebra.Utils;
21 import com.irurueta.geometry.Point3D;
22 import com.irurueta.navigation.frames.CoordinateTransformation;
23 import com.irurueta.navigation.frames.ECEFFrame;
24 import com.irurueta.navigation.frames.ECEFPosition;
25 import com.irurueta.navigation.frames.ECEFVelocity;
26 import com.irurueta.navigation.geodesic.Constants;
27 import com.irurueta.navigation.inertial.BodyKinematics;
28 import com.irurueta.units.Speed;
29 import com.irurueta.units.SpeedConverter;
30 import com.irurueta.units.SpeedUnit;
31 import com.irurueta.units.Time;
32 import com.irurueta.units.TimeConverter;
33 import com.irurueta.units.TimeUnit;
34
35 /**
36 * Estimates body kinematics (specific force applied to a body and its angular rates) with respect and resolved
37 * along ECEF-frame axes.
38 * This implementation is based on the equations defined in "Principles of GNSS, Inertial, and Multisensor
39 * Integrated Navigation Systems, Second Edition" and on the companion software available at:
40 * <a href="https://github.com/ymjdz/MATLAB-Codes/blob/master/Kinematics_ECEF.m">
41 * https://github.com/ymjdz/MATLAB-Codes/blob/master/Kinematics_ECEF.m
42 * </a>
43 */
44 public class ECEFKinematicsEstimator {
45
46 /**
47 * Earth rotation rate expressed in radians per second (rad/s).
48 */
49 public static final double EARTH_ROTATION_RATE = Constants.EARTH_ROTATION_RATE;
50
51 /**
52 * Scaling threshold.
53 */
54 private static final double SCALING_THRESHOLD = 2e-5;
55
56 /**
57 * Alpha threshold.
58 */
59 private static final double ALPHA_THRESHOLD = 1e-8;
60
61 /**
62 * Number of rows.
63 */
64 private static final int ROWS = 3;
65
66 /**
67 * Estimates body kinematics (specific force applied to a body and its angular rates).
68 *
69 * @param timeInterval time interval between epochs expressed in seconds (s).
70 * @param c body-to-ECEF frame coordinate transformation matrix.
71 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
72 * @param vx velocity of body frame x coordinate with respect ECEF frame,
73 * resolved along ECEF-frame axes and expressed in meters per
74 * second (m/s).
75 * @param vy velocity of body frame y coordinate with respect ECEF frame,
76 * resolved along ECEF-frame axes and expressed in meters per
77 * second (m/s).
78 * @param vz velocity of body frame z coordinate with respect ECEF frame,
79 * resolved along ECEF-frame axes and expressed in meters per
80 * second (m/s).
81 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
82 * frame, resolved along ECEF-frame axes and expressed in meters
83 * per second (m/s).
84 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
85 * frame, resolved along ECEF-frame axes and expressed in meters
86 * per second (m/s).
87 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
88 * frame, resolved along ECEF-frame axes and expressed in meters
89 * per second (m/s).
90 * @param x cartesian x coordinate of body position expressed in meters (m)
91 * with respect ECEF frame, resolved along ECEF-frame axes.
92 * @param y cartesian y coordinate of body position expressed in meters (m)
93 * with respect ECEF frame, resolved along ECEF-frame axes.
94 * @param z cartesian z coordinate of body position expressed in meters (m)
95 * with respect ECEF frame, resolved along ECEF-frame axes.
96 * @param result instance where body kinematics estimation will be stored.
97 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
98 * are not ECEF frame valid.
99 */
100 public void estimate(
101 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
102 final double vx, final double vy, final double vz,
103 final double oldVx, final double oldVy, final double oldVz,
104 final double x, final double y, final double z, final BodyKinematics result) {
105 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z, result);
106 }
107
108 /**
109 * Estimates body kinematics (specific force applied to a body and its angular rates).
110 *
111 * @param timeInterval time interval between epochs.
112 * @param c boy-to-ECEF frame coordinate transformation matrix.
113 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
114 * @param vx velocity of body frame x coordinate with respect ECEF frame,
115 * resolved along ECEF-frame axes and expressed in meters per
116 * second (m/s).
117 * @param vy velocity of body frame y coordinate with respect ECEF frame,
118 * resolved along ECEF-frame axes and expressed in meters per
119 * second (m/s).
120 * @param vz velocity of body frame z coordinate with respect ECEF frame,
121 * resolved along ECEF-frame axes and expressed in meters per
122 * second (m/s).
123 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
124 * frame, resolved along ECEF-frame axes and expressed in meters
125 * per second (m/s).
126 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
127 * frame, resolved along ECEF-frame axes and expressed in meters
128 * per second (m/s).
129 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
130 * frame, resolved along ECEF-frame axes and expressed in meters
131 * per second (m/s).
132 * @param x cartesian x coordinate of body position expressed in meters (m)
133 * with respect ECEF frame, resolved along ECEF-frame axes.
134 * @param y cartesian y coordinate of body position expressed in meters (m)
135 * with respect ECEF frame, resolved along ECEF-frame axes.
136 * @param z cartesian z coordinate of body position expressed in meters (m)
137 * with respect ECEF frame, resolved along ECEF-frame axes.
138 * @param result instance where body kinematics estimation will be stored.
139 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
140 * are not ECEF frame valid.
141 */
142 public void estimate(
143 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
144 final double vx, final double vy, final double vz,
145 final double oldVx, final double oldVy, final double oldVz,
146 final double x, final double y, final double z, final BodyKinematics result) {
147 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z, result);
148 }
149
150 /**
151 * Estimates body kinematics (specific force applied to a body and its angular rates).
152 *
153 * @param timeInterval time interval between epochs expressed in seconds (s).
154 * @param c boy-to-ECEF frame coordinate transformation matrix.
155 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
156 * @param velocity velocity of body frame with respect ECEF frame.
157 * @param oldVelocity previous velocity of body frame with respect ECEF frame.
158 * @param x cartesian x coordinate of body position expressed in meters (m)
159 * with respect ECEF frame, resolved along ECEF-frame axes.
160 * @param y cartesian y coordinate of body position expressed in meters (m)
161 * with respect ECEF frame, resolved along ECEF-frame axes.
162 * @param z cartesian z coordinate of body position expressed in meters (m)
163 * with respect ECEF frame, resolved along ECEF-frame axes.
164 * @param result instance where body kinematics estimation will be stored.
165 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
166 * are not ECEF frame valid.
167 */
168 public void estimate(
169 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
170 final ECEFVelocity velocity, final ECEFVelocity oldVelocity, final double x, final double y, final double z,
171 final BodyKinematics result) {
172 estimateKinematics(timeInterval, c, oldC, velocity, oldVelocity, x, y, z, result);
173 }
174
175 /**
176 * Estimates body kinematics (specific force applied to a body and its angular rates).
177 *
178 * @param timeInterval time interval between epochs.
179 * @param c boy-to-ECEF frame coordinate transformation matrix.
180 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
181 * @param velocity velocity of body frame with respect ECEF frame.
182 * @param oldVelocity previous velocity of body frame with respect ECEF frame.
183 * @param x cartesian x coordinate of body position expressed in meters (m)
184 * with respect ECEF frame, resolved along ECEF-frame axes.
185 * @param y cartesian y coordinate of body position expressed in meters (m)
186 * with respect ECEF frame, resolved along ECEF-frame axes.
187 * @param z cartesian z coordinate of body position expressed in meters (m)
188 * with respect ECEF frame, resolved along ECEF-frame axes.
189 * @param result instance where body kinematics estimation will be stored.
190 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
191 * are not ECEF frame valid.
192 */
193 public void estimate(
194 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
195 final ECEFVelocity velocity, final ECEFVelocity oldVelocity, final double x, final double y, final double z,
196 final BodyKinematics result) {
197 estimateKinematics(timeInterval, c, oldC, velocity, oldVelocity, x, y, z, result);
198 }
199
200 /**
201 * Estimates body kinematics (specific force applied to a body and its angular rates)..
202 *
203 * @param timeInterval time interval between epochs expressed in seconds (s).
204 * @param c body-to-ECEF-frame coordinate transformation.
205 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
206 * @param vx x coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
207 * along ECEF-frame axes.
208 * @param vy y coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
209 * along ECEF-frame axes.
210 * @param vz z coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
211 * along ECEF-frame axes.
212 * @param oldVx x coordinate of previous velocity of body frame expressed in meters per second (m/s) and
213 * resolved along ECEF-frame axes.
214 * @param oldVy y coordinate of previous velocity of body frame expressed in meters per second (m/s) and
215 * resolved along ECEF-frame axes.
216 * @param oldVz z coordinate of previous velocity of body frame expressed in meters per second (m/s) and
217 * resolved along ECEF-frame axes.
218 * @param position cartesian body position resolved along ECEF-frame axes.
219 * @param result instance where estimated body kinematics will be stored.
220 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
221 * are not ECEF frame valid.
222 */
223 public void estimate(
224 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
225 final double vx, final double vy, final double vz,
226 final double oldVx, final double oldVy, final double oldVz, final ECEFPosition position,
227 final BodyKinematics result) {
228 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position, result);
229 }
230
231 /**
232 * Estimates body kinematics (specific force applied to a body and its angular rates).
233 *
234 * @param timeInterval time interval between epochs.
235 * @param c body-to-ECEF-frame coordinate transformation.
236 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
237 * @param vx x coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
238 * along ECEF-frame axes.
239 * @param vy y coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
240 * along ECEF-frame axes.
241 * @param vz z coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
242 * along ECEF-frame axes.
243 * @param oldVx x coordinate of previous velocity of body frame expressed in meters per second (m/s) and
244 * resolved along ECEF-frame axes.
245 * @param oldVy y coordinate of previous velocity of body frame expressed in meters per second (m/s) and
246 * resolved along ECEF-frame axes.
247 * @param oldVz z coordinate of previous velocity of body frame expressed in meters per second (m/s) and
248 * resolved along ECEF-frame axes.
249 * @param position cartesian body position resolved along ECEF-frame axes.
250 * @param result instance where estimated body kinematics will be stored.
251 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
252 * are not ECEF frame valid.
253 */
254 public void estimate(
255 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
256 final double vx, final double vy, final double vz,
257 final double oldVx, final double oldVy, final double oldVz, final ECEFPosition position,
258 final BodyKinematics result) {
259 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position, result);
260 }
261
262 /**
263 * Estimates body kinematics (specific force applied to a body and its angular rates).
264 *
265 * @param timeInterval time interval between epochs expressed in seconds (s).
266 * @param c boy-to-ECEF frame coordinate transformation matrix.
267 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
268 * @param velocity velocity of body frame with respect ECEF frame.
269 * @param oldVelocity previous velocity of body frame with respect ECEF frame.
270 * @param position cartesian body position resolved along ECEF-frame axes.
271 * @param result instance where body kinematics estimation will be stored.
272 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
273 * are not ECEF frame valid.
274 */
275 public void estimate(
276 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
277 final ECEFVelocity velocity, final ECEFVelocity oldVelocity, final ECEFPosition position,
278 final BodyKinematics result) {
279 estimateKinematics(timeInterval, c, oldC, velocity, oldVelocity, position, result);
280 }
281
282 /**
283 * Estimates body kinematics (specific force applied to a body and its angular rates).
284 *
285 * @param timeInterval time interval between epochs.
286 * @param c boy-to-ECEF frame coordinate transformation matrix.
287 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
288 * @param velocity velocity of body frame with respect ECEF frame.
289 * @param oldVelocity previous velocity of body frame with respect ECEF frame.
290 * @param position cartesian body position resolved along ECEF-frame axes.
291 * @param result instance where body kinematics estimation will be stored.
292 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
293 * are not ECEF frame valid.
294 */
295 public void estimate(
296 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
297 final ECEFVelocity velocity, final ECEFVelocity oldVelocity, final ECEFPosition position,
298 final BodyKinematics result) {
299 estimateKinematics(timeInterval, c, oldC, velocity, oldVelocity, position, result);
300 }
301
302 /**
303 * Estimates body kinematics (specific force applied to a body and its angular rates).
304 *
305 * @param timeInterval time interval between epochs expressed in seconds (s).
306 * @param frame body ECEF frame containing current position, velocity and
307 * body-to-ECEF frame coordinate transformation.
308 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
309 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
310 * frame, resolved along ECEF-frame axes and expressed in meters
311 * per second (m/s).
312 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
313 * frame, resolved along ECEF-frame axes and expressed in meters
314 * per second (m/s).
315 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
316 * frame, resolved along ECEF-frame axes and expressed in meters
317 * per second (m/s).
318 * @param result instance where body kinematics estimation will be stored.
319 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
320 * are not ECEF frame valid.
321 */
322 public void estimate(
323 final double timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
324 final double oldVx, final double oldVy, final double oldVz, final BodyKinematics result) {
325 estimateKinematics(timeInterval, frame, oldC, oldVx, oldVy, oldVz, result);
326 }
327
328 /**
329 * Estimates body kinematics (specific force applied to a body and its angular rates).
330 *
331 * @param timeInterval time interval between epochs.
332 * @param frame body ECEF frame containing current position, velocity and
333 * body-to-ECEF frame coordinate transformation.
334 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
335 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
336 * frame, resolved along ECEF-frame axes and expressed in meters
337 * per second (m/s).
338 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
339 * frame, resolved along ECEF-frame axes and expressed in meters
340 * per second (m/s).
341 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
342 * frame, resolved along ECEF-frame axes and expressed in meters
343 * per second (m/s).
344 * @param result instance where body kinematics estimation will be stored.
345 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
346 * are not ECEF frame valid.
347 */
348 public void estimate(
349 final Time timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
350 final double oldVx, final double oldVy, final double oldVz, final BodyKinematics result) {
351 estimateKinematics(timeInterval, frame, oldC, oldVx, oldVy, oldVz, result);
352 }
353
354 /**
355 * Estimates body kinematics (specific force applied to a body and its angular rates).
356 *
357 * @param timeInterval time interval between epochs expressed in seconds (s).
358 * @param frame body ECEF frame containing current position, velocity and
359 * body-to-ECEF frame coordinate transformation.
360 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
361 * @param oldVelocity previous velocity of body frame with respect ECEF frame.
362 * @param result instance where body kinematics estimation will be stored.
363 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
364 * are not ECEF frame valid.
365 */
366 public void estimate(
367 final double timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
368 final ECEFVelocity oldVelocity, final BodyKinematics result) {
369 estimateKinematics(timeInterval, frame, oldC, oldVelocity, result);
370 }
371
372 /**
373 * Estimates body kinematics (specific force applied to a body and its angular rates).
374 *
375 * @param timeInterval time interval between epochs.
376 * @param frame body ECEF frame containing current position, velocity and
377 * body-to-ECEF frame coordinate transformation.
378 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
379 * @param oldVelocity previous velocity of body frame with respect ECEF frame.
380 * @param result instance where body kinematics estimation will be stored.
381 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
382 * are not ECEF frame valid.
383 */
384 public void estimate(
385 final Time timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
386 final ECEFVelocity oldVelocity, final BodyKinematics result) {
387 estimateKinematics(timeInterval, frame, oldC, oldVelocity, result);
388 }
389
390 /**
391 * Estimates body kinematics (specific force applied to a body and its angular rates).
392 *
393 * @param timeInterval time interval between epochs expressed in seconds (s).
394 * @param frame body ECEF frame containing current position, velocity and
395 * body-to-ECEF frame coordinate transformation.
396 * @param oldFrame body ECEF frame containing previous position, velocity and
397 * body-to-ECEF frame coordinate transformation. Notice that
398 * previous position contained in this frame is ignored.
399 * @param result instance where body kinematics estimation will be stored.
400 * @throws IllegalArgumentException if provided time interval is negative.
401 */
402 public void estimate(
403 final double timeInterval, final ECEFFrame frame, final ECEFFrame oldFrame, final BodyKinematics result) {
404 estimateKinematics(timeInterval, frame, oldFrame, result);
405 }
406
407 /**
408 * Estimates body kinematics (specific force applied to a body and its angular rates).
409 *
410 * @param timeInterval time interval between epochs.
411 * @param frame body ECEF frame containing current position, velocity and
412 * body-to-ECEF frame coordinate transformation.
413 * @param oldFrame body ECEF frame containing previous position, velocity and
414 * body-to-ECEF frame coordinate transformation. Notice that
415 * previous position contained in this frame is ignored.
416 * @param result instance where body kinematics estimation will be stored.
417 * @throws IllegalArgumentException if provided time interval is negative.
418 */
419 public void estimate(
420 final Time timeInterval, final ECEFFrame frame, final ECEFFrame oldFrame, final BodyKinematics result) {
421 estimateKinematics(timeInterval, frame, oldFrame, result);
422 }
423
424 /**
425 * Estimates body kinematics (specific force applied to a body and its angular rates).
426 *
427 * @param timeInterval time interval between epochs expressed in seconds (s).
428 * @param c body-to-ECEF frame coordinate transformation matrix.
429 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
430 * @param vx velocity of body frame x coordinate with respect ECEF frame,
431 * resolved along ECEF-frame axes.
432 * @param vy velocity of body frame y coordinate with respect ECEF frame,
433 * resolved along ECEF-frame axes.
434 * @param vz velocity of body frame z coordinate with respect ECEF frame,
435 * resolved along ECEF-frame axes.
436 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
437 * frame, resolved along ECEF-frame axes.
438 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
439 * frame, resolved along ECEF-frame axes.
440 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
441 * frame, resolved along ECEF-frame axes.
442 * @param x cartesian x coordinate of body position expressed in meters (m)
443 * with respect ECEF frame, resolved along ECEF-frame axes.
444 * @param y cartesian y coordinate of body position expressed in meters (m)
445 * with respect ECEF frame, resolved along ECEF-frame axes.
446 * @param z cartesian z coordinate of body position expressed in meters (m)
447 * with respect ECEF frame, resolved along ECEF-frame axes.
448 * @param result instance where body kinematics estimation will be stored.
449 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
450 * are not ECEF frame valid.
451 */
452 public void estimate(
453 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
454 final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
455 final double x, final double y, final double z, final BodyKinematics result) {
456 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z, result);
457 }
458
459 /**
460 * Estimates body kinematics (specific force applied to a body and its angular rates).
461 *
462 * @param timeInterval time interval between epochs.
463 * @param c body-to-ECEF frame coordinate transformation matrix.
464 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
465 * @param vx velocity of body frame x coordinate with respect ECEF frame,
466 * resolved along ECEF-frame axes.
467 * @param vy velocity of body frame y coordinate with respect ECEF frame,
468 * resolved along ECEF-frame axes.
469 * @param vz velocity of body frame z coordinate with respect ECEF frame,
470 * resolved along ECEF-frame axes.
471 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
472 * frame, resolved along ECEF-frame axes.
473 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
474 * frame, resolved along ECEF-frame axes.
475 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
476 * frame, resolved along ECEF-frame axes.
477 * @param x cartesian x coordinate of body position expressed in meters (m)
478 * with respect ECEF frame, resolved along ECEF-frame axes.
479 * @param y cartesian y coordinate of body position expressed in meters (m)
480 * with respect ECEF frame, resolved along ECEF-frame axes.
481 * @param z cartesian z coordinate of body position expressed in meters (m)
482 * with respect ECEF frame, resolved along ECEF-frame axes.
483 * @param result instance where body kinematics estimation will be stored.
484 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
485 * are not ECEF frame valid.
486 */
487 public void estimate(
488 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
489 final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
490 final double x, final double y, final double z, final BodyKinematics result) {
491 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z, result);
492 }
493
494 /**
495 * Estimates body kinematics (specific force applied to a body and its angular rates).
496 *
497 * @param timeInterval time interval between epochs expressed in seconds (s).
498 * @param c body-to-ECEF frame coordinate transformation matrix.
499 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
500 * @param vx velocity of body frame x coordinate with respect ECEF frame,
501 * resolved along ECEF-frame axes.
502 * @param vy velocity of body frame y coordinate with respect ECEF frame,
503 * resolved along ECEF-frame axes.
504 * @param vz velocity of body frame z coordinate with respect ECEF frame,
505 * resolved along ECEF-frame axes.
506 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
507 * frame, resolved along ECEF-frame axes.
508 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
509 * frame, resolved along ECEF-frame axes.
510 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
511 * frame, resolved along ECEF-frame axes.
512 * @param position cartesian body position resolved along ECEF-frame axes.
513 * @param result instance where body kinematics estimation will be stored.
514 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
515 * are not ECEF frame valid.
516 */
517 public void estimate(
518 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
519 final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
520 final ECEFPosition position, final BodyKinematics result) {
521 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position, result);
522 }
523
524 /**
525 * Estimates body kinematics (specific force applied to a body and its angular rates).
526 *
527 * @param timeInterval time interval between epochs.
528 * @param c body-to-ECEF frame coordinate transformation matrix.
529 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
530 * @param vx velocity of body frame x coordinate with respect ECEF frame,
531 * resolved along ECEF-frame axes.
532 * @param vy velocity of body frame y coordinate with respect ECEF frame,
533 * resolved along ECEF-frame axes.
534 * @param vz velocity of body frame z coordinate with respect ECEF frame,
535 * resolved along ECEF-frame axes.
536 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
537 * frame, resolved along ECEF-frame axes.
538 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
539 * frame, resolved along ECEF-frame axes.
540 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
541 * frame, resolved along ECEF-frame axes.
542 * @param position cartesian body position resolved along ECEF-frame axes.
543 * @param result instance where body kinematics estimation will be stored.
544 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
545 * are not ECEF frame valid.
546 */
547 public void estimate(
548 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
549 final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
550 final ECEFPosition position, final BodyKinematics result) {
551 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position, result);
552 }
553
554 /**
555 * Estimates body kinematics (specific force applied to a body and its angular rates).
556 *
557 * @param timeInterval time interval between epochs expressed in seconds (s).
558 * @param frame body ECEF frame containing current position, velocity and
559 * body-to-ECEF frame coordinate transformation.
560 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
561 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
562 * frame, resolved along ECEF-frame axes.
563 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
564 * frame, resolved along ECEF-frame axes.
565 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
566 * frame, resolved along ECEF-frame axes.
567 * @param result instance where body kinematics estimation will be stored.
568 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
569 * are not ECEF frame valid.
570 */
571 public void estimate(
572 final double timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
573 final Speed oldVx, final Speed oldVy, final Speed oldVz, final BodyKinematics result) {
574 estimateKinematics(timeInterval, frame, oldC, oldVx, oldVy, oldVz, result);
575 }
576
577 /**
578 * Estimates body kinematics (specific force applied to a body and its angular rates).
579 *
580 * @param timeInterval time interval between epochs.
581 * @param frame body ECEF frame containing current position, velocity and
582 * body-to-ECEF frame coordinate transformation.
583 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
584 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
585 * frame, resolved along ECEF-frame axes.
586 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
587 * frame, resolved along ECEF-frame axes.
588 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
589 * frame, resolved along ECEF-frame axes.
590 * @param result instance where body kinematics estimation will be stored.
591 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
592 * are not ECEF frame valid.
593 */
594 public void estimate(
595 final Time timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
596 final Speed oldVx, final Speed oldVy, final Speed oldVz, final BodyKinematics result) {
597 estimateKinematics(timeInterval, frame, oldC, oldVx, oldVy, oldVz, result);
598 }
599
600 /**
601 * Estimates body kinematics (specific force applied to a body and its angular rates).
602 *
603 * @param timeInterval time interval between epochs expressed in seconds (s).
604 * @param c body-to-ECEF frame coordinate transformation matrix.
605 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
606 * @param vx velocity of body frame x coordinate with respect ECEF frame,
607 * resolved along ECEF-frame axes and expressed in meters per
608 * second (m/s).
609 * @param vy velocity of body frame y coordinate with respect ECEF frame,
610 * resolved along ECEF-frame axes and expressed in meters per
611 * second (m/s).
612 * @param vz velocity of body frame z coordinate with respect ECEF frame,
613 * resolved along ECEF-frame axes and expressed in meters per
614 * second (m/s).
615 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
616 * frame, resolved along ECEF-frame axes and expressed in meters
617 * per second (m/s).
618 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
619 * frame, resolved along ECEF-frame axes and expressed in meters
620 * per second (m/s).
621 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
622 * frame, resolved along ECEF-frame axes and expressed in meters
623 * per second (m/s).
624 * @param position body position expressed in meters (m) with respect ECEF frame,
625 * resolved along ECEF-frame axes.
626 * @param result instance where body kinematics estimation will be stored.
627 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
628 * are not ECEF frame valid.
629 */
630 public void estimate(
631 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
632 final double vx, final double vy, final double vz,
633 final double oldVx, final double oldVy, final double oldVz, final Point3D position,
634 final BodyKinematics result) {
635 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position, result);
636 }
637
638 /**
639 * Estimates body kinematics (specific force applied to a body and its angular rates).
640 *
641 * @param timeInterval time interval between epochs.
642 * @param c body-to-ECEF frame coordinate transformation matrix.
643 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
644 * @param vx velocity of body frame x coordinate with respect ECEF frame,
645 * resolved along ECEF-frame axes and expressed in meters per
646 * second (m/s).
647 * @param vy velocity of body frame y coordinate with respect ECEF frame,
648 * resolved along ECEF-frame axes and expressed in meters per
649 * second (m/s).
650 * @param vz velocity of body frame z coordinate with respect ECEF frame,
651 * resolved along ECEF-frame axes and expressed in meters per
652 * second (m/s).
653 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
654 * frame, resolved along ECEF-frame axes and expressed in meters
655 * per second (m/s).
656 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
657 * frame, resolved along ECEF-frame axes and expressed in meters
658 * per second (m/s).
659 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
660 * frame, resolved along ECEF-frame axes and expressed in meters
661 * per second (m/s).
662 * @param position body position expressed in meters (m) with respect ECEF frame,
663 * resolved along ECEF-frame axes.
664 * @param result instance where body kinematics estimation will be stored.
665 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
666 * are not ECEF frame valid.
667 */
668 public void estimate(
669 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
670 final double vx, final double vy, final double vz,
671 final double oldVx, final double oldVy, final double oldVz, final Point3D position,
672 final BodyKinematics result) {
673 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position, result);
674 }
675
676 /**
677 * Estimates body kinematics (specific force applied to a body and its angular rates).
678 *
679 * @param timeInterval time interval between epochs expressed in seconds (s).
680 * @param c body-to-ECEF frame coordinate transformation matrix.
681 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
682 * @param velocity velocity of body frame with respect ECEF frame.
683 * @param oldVelocity previous velocity of body frame with respect ECEF frame.
684 * @param position body position expressed in meters (m) with respect ECEF frame,
685 * resolved along ECEF-frame axes.
686 * @param result instance where body kinematics estimation will be stored.
687 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
688 * are not ECEF frame valid.
689 */
690 public void estimate(
691 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
692 final ECEFVelocity velocity, final ECEFVelocity oldVelocity, final Point3D position,
693 final BodyKinematics result) {
694 estimateKinematics(timeInterval, c, oldC, velocity, oldVelocity, position, result);
695 }
696
697 /**
698 * Estimates body kinematics (specific force applied to a body and its angular rates).
699 *
700 * @param timeInterval time interval between epochs.
701 * @param c body-to-ECEF frame coordinate transformation matrix.
702 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
703 * @param velocity velocity of body frame with respect ECEF frame.
704 * @param oldVelocity previous velocity of body frame with respect ECEF frame.
705 * @param position body position expressed in meters (m) with respect ECEF frame,
706 * resolved along ECEF-frame axes.
707 * @param result instance where body kinematics estimation will be stored.
708 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
709 * are not ECEF frame valid.
710 */
711 public void estimate(
712 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
713 final ECEFVelocity velocity, final ECEFVelocity oldVelocity, final Point3D position,
714 final BodyKinematics result) {
715 estimateKinematics(timeInterval, c, oldC, velocity, oldVelocity, position, result);
716 }
717
718 /**
719 * Estimates body kinematics (specific force applied to a body and its angular rates).
720 *
721 * @param timeInterval time interval between epochs expressed in seconds (s).
722 * @param c boy-to-ECEF frame coordinate transformation matrix.
723 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
724 * @param vx velocity of body frame x coordinate with respect ECEF frame,
725 * resolved along ECEF-frame axes.
726 * @param vy velocity of body frame y coordinate with respect ECEF frame,
727 * resolved along ECEF-frame axes.
728 * @param vz velocity of body frame z coordinate with respect ECEF frame,
729 * resolved along ECEF-frame axes.
730 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
731 * frame, resolved along ECEF-frame axes.
732 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
733 * frame, resolved along ECEF-frame axes.
734 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
735 * frame, resolved along ECEF-frame axes.
736 * @param position body position expressed in meters (m) with respect ECEF frame,
737 * resolved along ECEF-frame axes.
738 * @param result instance where body kinematics estimation will be stored.
739 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
740 * are not ECEF frame valid.
741 */
742 public void estimate(
743 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
744 final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
745 final Point3D position, final BodyKinematics result) {
746 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position, result);
747 }
748
749 /**
750 * Estimates body kinematics (specific force applied to a body and its angular rates).
751 *
752 * @param timeInterval time interval between epochs.
753 * @param c boy-to-ECEF frame coordinate transformation matrix.
754 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
755 * @param vx velocity of body frame x coordinate with respect ECEF frame,
756 * resolved along ECEF-frame axes.
757 * @param vy velocity of body frame y coordinate with respect ECEF frame,
758 * resolved along ECEF-frame axes.
759 * @param vz velocity of body frame z coordinate with respect ECEF frame,
760 * resolved along ECEF-frame axes.
761 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
762 * frame, resolved along ECEF-frame axes.
763 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
764 * frame, resolved along ECEF-frame axes.
765 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
766 * frame, resolved along ECEF-frame axes.
767 * @param position body position expressed in meters (m) with respect ECEF frame,
768 * resolved along ECEF-frame axes.
769 * @param result instance where body kinematics estimation will be stored.
770 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
771 * are not ECEF frame valid.
772 */
773 public void estimate(
774 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
775 final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
776 final Point3D position, final BodyKinematics result) {
777 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position, result);
778 }
779
780 /**
781 * Estimates body kinematics (specific force applied to a body and its angular rates).
782 *
783 * @param timeInterval time interval between epochs expressed in seconds (s).
784 * @param c body-to-ECEF frame coordinate transformation matrix.
785 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
786 * @param vx velocity of body frame x coordinate with respect ECEF frame,
787 * resolved along ECEF-frame axes and expressed in meters per
788 * second (m/s).
789 * @param vy velocity of body frame y coordinate with respect ECEF frame,
790 * resolved along ECEF-frame axes and expressed in meters per
791 * second (m/s).
792 * @param vz velocity of body frame z coordinate with respect ECEF frame,
793 * resolved along ECEF-frame axes and expressed in meters per
794 * second (m/s).
795 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
796 * frame, resolved along ECEF-frame axes and expressed in meters
797 * per second (m/s).
798 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
799 * frame, resolved along ECEF-frame axes and expressed in meters
800 * per second (m/s).
801 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
802 * frame, resolved along ECEF-frame axes and expressed in meters
803 * per second (m/s).
804 * @param x cartesian x coordinate of body position expressed in meters (m)
805 * with respect ECEF frame, resolved along ECEF-frame axes.
806 * @param y cartesian y coordinate of body position expressed in meters (m)
807 * with respect ECEF frame, resolved along ECEF-frame axes.
808 * @param z cartesian z coordinate of body position expressed in meters (m)
809 * with respect ECEF frame, resolved along ECEF-frame axes.
810 * @return a new body kinematics instance.
811 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
812 * are not ECEF frame valid.
813 */
814 public BodyKinematics estimateAndReturnNew(
815 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
816 final double vx, final double vy, final double vz,
817 final double oldVx, final double oldVy, final double oldVz,
818 final double x, final double y, final double z) {
819 return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z);
820 }
821
822 /**
823 * Estimates body kinematics (specific force applied to a body and its angular rates).
824 *
825 * @param timeInterval time interval between.
826 * @param c body-to-ECEF frame coordinate transformation matrix.
827 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
828 * @param vx velocity of body frame x coordinate with respect ECEF frame,
829 * resolved along ECEF-frame axes and expressed in meters per
830 * second (m/s).
831 * @param vy velocity of body frame y coordinate with respect ECEF frame,
832 * resolved along ECEF-frame axes and expressed in meters per
833 * second (m/s).
834 * @param vz velocity of body frame z coordinate with respect ECEF frame,
835 * resolved along ECEF-frame axes and expressed in meters per
836 * second (m/s).
837 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
838 * frame, resolved along ECEF-frame axes and expressed in meters
839 * per second (m/s).
840 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
841 * frame, resolved along ECEF-frame axes and expressed in meters
842 * per second (m/s).
843 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
844 * frame, resolved along ECEF-frame axes and expressed in meters
845 * per second (m/s).
846 * @param x cartesian x coordinate of body position expressed in meters (m)
847 * with respect ECEF frame, resolved along ECEF-frame axes.
848 * @param y cartesian y coordinate of body position expressed in meters (m)
849 * with respect ECEF frame, resolved along ECEF-frame axes.
850 * @param z cartesian z coordinate of body position expressed in meters (m)
851 * with respect ECEF frame, resolved along ECEF-frame axes.
852 * @return a new body kinematics instance.
853 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
854 * are not ECEF frame valid.
855 */
856 public BodyKinematics estimateAndReturnNew(
857 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
858 final double vx, final double vy, final double vz,
859 final double oldVx, final double oldVy, final double oldVz,
860 final double x, final double y, final double z) {
861 return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z);
862 }
863
864 /**
865 * Estimates body kinematics (specific force applied to a body and its angular rates).
866 *
867 * @param timeInterval time interval between epochs expressed in seconds (s).
868 * @param c boy-to-ECEF frame coordinate transformation matrix.
869 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
870 * @param velocity velocity of body frame with respect ECEF frame.
871 * @param oldVelocity previous velocity of body frame with respect ECEF frame.
872 * @param x cartesian x coordinate of body position expressed in meters (m)
873 * with respect ECEF frame, resolved along ECEF-frame axes.
874 * @param y cartesian y coordinate of body position expressed in meters (m)
875 * with respect ECEF frame, resolved along ECEF-frame axes.
876 * @param z cartesian z coordinate of body position expressed in meters (m)
877 * with respect ECEF frame, resolved along ECEF-frame axes.
878 * @return a new body kinematics instance.
879 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
880 * are not ECEF frame valid.
881 */
882 public BodyKinematics estimateAndReturnNew(
883 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
884 final ECEFVelocity velocity, final ECEFVelocity oldVelocity,
885 final double x, final double y, final double z) {
886 return estimateKinematicsAndReturnNew(timeInterval, c, oldC, velocity, oldVelocity, x, y, z);
887 }
888
889 /**
890 * Estimates body kinematics (specific force applied to a body and its angular rates).
891 *
892 * @param timeInterval time interval between epochs.
893 * @param c boy-to-ECEF frame coordinate transformation matrix.
894 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
895 * @param velocity velocity of body frame with respect ECEF frame.
896 * @param oldVelocity previous velocity of body frame with respect ECEF frame.
897 * @param x cartesian x coordinate of body position expressed in meters (m)
898 * with respect ECEF frame, resolved along ECEF-frame axes.
899 * @param y cartesian y coordinate of body position expressed in meters (m)
900 * with respect ECEF frame, resolved along ECEF-frame axes.
901 * @param z cartesian z coordinate of body position expressed in meters (m)
902 * with respect ECEF frame, resolved along ECEF-frame axes.
903 * @return a new body kinematics instance.
904 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
905 * are not ECEF frame valid.
906 */
907 public BodyKinematics estimateAndReturnNew(
908 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
909 final ECEFVelocity velocity, final ECEFVelocity oldVelocity,
910 final double x, final double y, final double z) {
911 return estimateKinematicsAndReturnNew(timeInterval, c, oldC, velocity, oldVelocity, x, y, z);
912 }
913
914 /**
915 * Estimates body kinematics (specific force applied to a body and its angular rates)..
916 *
917 * @param timeInterval time interval between epochs expressed in seconds (s).
918 * @param c body-to-ECEF-frame coordinate transformation.
919 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
920 * @param vx x coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
921 * along ECEF-frame axes.
922 * @param vy y coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
923 * along ECEF-frame axes.
924 * @param vz z coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
925 * along ECEF-frame axes.
926 * @param oldVx x coordinate of previous velocity of body frame expressed in meters per second (m/s) and
927 * resolved along ECEF-frame axes.
928 * @param oldVy y coordinate of previous velocity of body frame expressed in meters per second (m/s) and
929 * resolved along ECEF-frame axes.
930 * @param oldVz z coordinate of previous velocity of body frame expressed in meters per second (m/s) and
931 * resolved along ECEF-frame axes.
932 * @param position cartesian body position resolved along ECEF-frame axes.
933 * @return a new body kinematics instance.
934 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
935 * are not ECEF frame valid.
936 */
937 public BodyKinematics estimateAndReturnNew(
938 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
939 final double vx, final double vy, final double vz,
940 final double oldVx, final double oldVy, final double oldVz, final ECEFPosition position) {
941 return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position);
942 }
943
944 /**
945 * Estimates body kinematics (specific force applied to a body and its angular rates)..
946 *
947 * @param timeInterval time interval between epochs.
948 * @param c body-to-ECEF-frame coordinate transformation.
949 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
950 * @param vx x coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
951 * along ECEF-frame axes.
952 * @param vy y coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
953 * along ECEF-frame axes.
954 * @param vz z coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
955 * along ECEF-frame axes.
956 * @param oldVx x coordinate of previous velocity of body frame expressed in meters per second (m/s) and
957 * resolved along ECEF-frame axes.
958 * @param oldVy y coordinate of previous velocity of body frame expressed in meters per second (m/s) and
959 * resolved along ECEF-frame axes.
960 * @param oldVz z coordinate of previous velocity of body frame expressed in meters per second (m/s) and
961 * resolved along ECEF-frame axes.
962 * @param position cartesian body position resolved along ECEF-frame axes.
963 * @return a new body kinematics instance.
964 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
965 * are not ECEF frame valid.
966 */
967 public BodyKinematics estimateAndReturnNew(
968 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
969 final double vx, final double vy, final double vz,
970 final double oldVx, final double oldVy, final double oldVz, final ECEFPosition position) {
971 return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position);
972 }
973
974 /**
975 * Estimates body kinematics (specific force applied to a body and its angular rates).
976 *
977 * @param timeInterval time interval between epochs expressed in seconds (s).
978 * @param c boy-to-ECEF frame coordinate transformation matrix.
979 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
980 * @param velocity velocity of body frame with respect ECEF frame.
981 * @param oldVelocity previous velocity of body frame with respect ECEF frame.
982 * @param position cartesian body position resolved along ECEF-frame axes.
983 * @return a new body kinematics instance.
984 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
985 * are not ECEF frame valid.
986 */
987 public BodyKinematics estimateAndReturnNew(
988 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
989 final ECEFVelocity velocity, final ECEFVelocity oldVelocity, final ECEFPosition position) {
990 return estimateKinematicsAndReturnNew(timeInterval, c, oldC, velocity, oldVelocity, position);
991 }
992
993 /**
994 * Estimates body kinematics (specific force applied to a body and its angular rates).
995 *
996 * @param timeInterval time interval between epochs.
997 * @param c boy-to-ECEF frame coordinate transformation matrix.
998 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
999 * @param velocity velocity of body frame with respect ECEF frame.
1000 * @param oldVelocity previous velocity of body frame with respect ECEF frame.
1001 * @param position cartesian body position resolved along ECEF-frame axes.
1002 * @return a new body kinematics instance.
1003 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
1004 * are not ECEF frame valid.
1005 */
1006 public BodyKinematics estimateAndReturnNew(
1007 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
1008 final ECEFVelocity velocity, final ECEFVelocity oldVelocity, final ECEFPosition position) {
1009 return estimateKinematicsAndReturnNew(timeInterval, c, oldC, velocity, oldVelocity, position);
1010 }
1011
1012 /**
1013 * Estimates body kinematics (specific force applied to a body and its angular rates).
1014 *
1015 * @param timeInterval time interval between epochs expressed in seconds (s).
1016 * @param frame body ECEF frame containing current position, velocity and
1017 * body-to-ECEF frame coordinate transformation.
1018 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
1019 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
1020 * frame, resolved along ECEF-frame axes and expressed in meters
1021 * per second (m/s).
1022 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
1023 * frame, resolved along ECEF-frame axes and expressed in meters
1024 * per second (m/s).
1025 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
1026 * frame, resolved along ECEF-frame axes and expressed in meters
1027 * per second (m/s).
1028 * @return a new body kinematics instance.
1029 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
1030 * are not ECEF frame valid.
1031 */
1032 public BodyKinematics estimateAndReturnNew(
1033 final double timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
1034 final double oldVx, final double oldVy, final double oldVz) {
1035 return estimateKinematicsAndReturnNew(timeInterval, frame, oldC, oldVx, oldVy, oldVz);
1036 }
1037
1038 /**
1039 * Estimates body kinematics (specific force applied to a body and its angular rates).
1040 *
1041 * @param timeInterval time interval between epochs.
1042 * @param frame body ECEF frame containing current position, velocity and
1043 * body-to-ECEF frame coordinate transformation.
1044 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
1045 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
1046 * frame, resolved along ECEF-frame axes and expressed in meters
1047 * per second (m/s).
1048 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
1049 * frame, resolved along ECEF-frame axes and expressed in meters
1050 * per second (m/s).
1051 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
1052 * frame, resolved along ECEF-frame axes and expressed in meters
1053 * per second (m/s).
1054 * @return a new body kinematics instance.
1055 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
1056 * are not ECEF frame valid.
1057 */
1058 public BodyKinematics estimateAndReturnNew(
1059 final Time timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
1060 final double oldVx, final double oldVy, final double oldVz) {
1061 return estimateKinematicsAndReturnNew(timeInterval, frame, oldC, oldVx, oldVy, oldVz);
1062 }
1063
1064 /**
1065 * Estimates body kinematics (specific force applied to a body and its angular rates).
1066 *
1067 * @param timeInterval time interval between epochs expressed in seconds (s).
1068 * @param frame body ECEF frame containing current position, velocity and
1069 * body-to-ECEF frame coordinate transformation.
1070 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
1071 * @param oldVelocity previous velocity of body frame with respect ECEF frame.
1072 * @return a new body kinematics instance.
1073 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
1074 * are not ECEF frame valid.
1075 */
1076 public BodyKinematics estimateAndReturnNew(
1077 final double timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
1078 final ECEFVelocity oldVelocity) {
1079 return estimateKinematicsAndReturnNew(timeInterval, frame, oldC, oldVelocity);
1080 }
1081
1082 /**
1083 * Estimates body kinematics (specific force applied to a body and its angular rates).
1084 *
1085 * @param timeInterval time interval between epochs.
1086 * @param frame body ECEF frame containing current position, velocity and
1087 * body-to-ECEF frame coordinate transformation.
1088 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
1089 * @param oldVelocity previous velocity of body frame with respect ECEF frame.
1090 * @return a new body kinematics instance.
1091 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
1092 * are not ECEF frame valid.
1093 */
1094 public BodyKinematics estimateAndReturnNew(
1095 final Time timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
1096 final ECEFVelocity oldVelocity) {
1097 return estimateKinematicsAndReturnNew(timeInterval, frame, oldC, oldVelocity);
1098 }
1099
1100 /**
1101 * Estimates body kinematics (specific force applied to a body and its angular rates).
1102 *
1103 * @param timeInterval time interval between epochs expressed in seconds (s).
1104 * @param frame body ECEF frame containing current position, velocity and
1105 * body-to-ECEF frame coordinate transformation.
1106 * @param oldFrame body ECEF frame containing previous position, velocity and
1107 * body-to-ECEF frame coordinate transformation. Notice that
1108 * previous position contained in this frame is ignored.
1109 * @return a new body kinematics instance.
1110 * @throws IllegalArgumentException if provided time interval is negative.
1111 */
1112 public BodyKinematics estimateAndReturnNew(
1113 final double timeInterval, final ECEFFrame frame, final ECEFFrame oldFrame) {
1114 return estimateKinematicsAndReturnNew(timeInterval, frame, oldFrame);
1115 }
1116
1117 /**
1118 * Estimates body kinematics (specific force applied to a body and its angular rates).
1119 *
1120 * @param timeInterval time interval between epochs.
1121 * @param frame body ECEF frame containing current position, velocity and
1122 * body-to-ECEF frame coordinate transformation.
1123 * @param oldFrame body ECEF frame containing previous position, velocity and
1124 * body-to-ECEF frame coordinate transformation. Notice that
1125 * previous position contained in this frame is ignored.
1126 * @return a new body kinematics instance.
1127 * @throws IllegalArgumentException if provided time interval is negative.
1128 */
1129 public BodyKinematics estimateAndReturnNew(
1130 final Time timeInterval, final ECEFFrame frame, final ECEFFrame oldFrame) {
1131 return estimateKinematicsAndReturnNew(timeInterval, frame, oldFrame);
1132 }
1133
1134 /**
1135 * Estimates body kinematics (specific force applied to a body and its angular rates).
1136 *
1137 * @param timeInterval time interval between epochs expressed in seconds (s).
1138 * @param c body-to-ECEF frame coordinate transformation matrix.
1139 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
1140 * @param vx velocity of body frame x coordinate with respect ECEF frame,
1141 * resolved along ECEF-frame axes.
1142 * @param vy velocity of body frame y coordinate with respect ECEF frame,
1143 * resolved along ECEF-frame axes.
1144 * @param vz velocity of body frame z coordinate with respect ECEF frame,
1145 * resolved along ECEF-frame axes.
1146 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
1147 * frame, resolved along ECEF-frame axes.
1148 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
1149 * frame, resolved along ECEF-frame axes.
1150 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
1151 * frame, resolved along ECEF-frame axes.
1152 * @param x cartesian x coordinate of body position expressed in meters (m)
1153 * with respect ECEF frame, resolved along ECEF-frame axes.
1154 * @param y cartesian y coordinate of body position expressed in meters (m)
1155 * with respect ECEF frame, resolved along ECEF-frame axes.
1156 * @param z cartesian z coordinate of body position expressed in meters (m)
1157 * with respect ECEF frame, resolved along ECEF-frame axes.
1158 * @return a new body kinematics instance.
1159 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
1160 * are not ECEF frame valid.
1161 */
1162 public BodyKinematics estimateAndReturnNew(
1163 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
1164 final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
1165 final double x, final double y, final double z) {
1166 return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z);
1167 }
1168
1169 /**
1170 * Estimates body kinematics (specific force applied to a body and its angular rates).
1171 *
1172 * @param timeInterval time interval between epochs.
1173 * @param c body-to-ECEF frame coordinate transformation matrix.
1174 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
1175 * @param vx velocity of body frame x coordinate with respect ECEF frame,
1176 * resolved along ECEF-frame axes.
1177 * @param vy velocity of body frame y coordinate with respect ECEF frame,
1178 * resolved along ECEF-frame axes.
1179 * @param vz velocity of body frame z coordinate with respect ECEF frame,
1180 * resolved along ECEF-frame axes.
1181 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
1182 * frame, resolved along ECEF-frame axes.
1183 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
1184 * frame, resolved along ECEF-frame axes.
1185 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
1186 * frame, resolved along ECEF-frame axes.
1187 * @param x cartesian x coordinate of body position expressed in meters (m)
1188 * with respect ECEF frame, resolved along ECEF-frame axes.
1189 * @param y cartesian y coordinate of body position expressed in meters (m)
1190 * with respect ECEF frame, resolved along ECEF-frame axes.
1191 * @param z cartesian z coordinate of body position expressed in meters (m)
1192 * with respect ECEF frame, resolved along ECEF-frame axes.
1193 * @return a new body kinematics instance.
1194 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
1195 * are not ECEF frame valid.
1196 */
1197 public BodyKinematics estimateAndReturnNew(
1198 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
1199 final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
1200 final double x, final double y, final double z) {
1201 return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z);
1202 }
1203
1204 /**
1205 * Estimates body kinematics (specific force applied to a body and its angular rates).
1206 *
1207 * @param timeInterval time interval between epochs expressed in seconds (s).
1208 * @param c body-to-ECEF frame coordinate transformation matrix.
1209 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
1210 * @param vx velocity of body frame x coordinate with respect ECEF frame,
1211 * resolved along ECEF-frame axes.
1212 * @param vy velocity of body frame y coordinate with respect ECEF frame,
1213 * resolved along ECEF-frame axes.
1214 * @param vz velocity of body frame z coordinate with respect ECEF frame,
1215 * resolved along ECEF-frame axes.
1216 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
1217 * frame, resolved along ECEF-frame axes.
1218 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
1219 * frame, resolved along ECEF-frame axes.
1220 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
1221 * frame, resolved along ECEF-frame axes.
1222 * @param position cartesian body position resolved along ECEF-frame axes.
1223 * @return a new body kinematics instance.
1224 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
1225 * are not ECEF frame valid.
1226 */
1227 public BodyKinematics estimateAndReturnNew(
1228 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
1229 final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
1230 final ECEFPosition position) {
1231 return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position);
1232 }
1233
1234 /**
1235 * Estimates body kinematics (specific force applied to a body and its angular rates).
1236 *
1237 * @param timeInterval time interval between epochs.
1238 * @param c body-to-ECEF frame coordinate transformation matrix.
1239 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
1240 * @param vx velocity of body frame x coordinate with respect ECEF frame,
1241 * resolved along ECEF-frame axes.
1242 * @param vy velocity of body frame y coordinate with respect ECEF frame,
1243 * resolved along ECEF-frame axes.
1244 * @param vz velocity of body frame z coordinate with respect ECEF frame,
1245 * resolved along ECEF-frame axes.
1246 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
1247 * frame, resolved along ECEF-frame axes.
1248 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
1249 * frame, resolved along ECEF-frame axes.
1250 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
1251 * frame, resolved along ECEF-frame axes.
1252 * @param position cartesian body position resolved along ECEF-frame axes.
1253 * @return a new body kinematics instance.
1254 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
1255 * are not ECEF frame valid.
1256 */
1257 public BodyKinematics estimateAndReturnNew(
1258 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
1259 final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
1260 final ECEFPosition position) {
1261 return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position);
1262 }
1263
1264 /**
1265 * Estimates body kinematics (specific force applied to a body and its angular rates).
1266 *
1267 * @param timeInterval time interval between epochs expressed in seconds (s).
1268 * @param frame body ECEF frame containing current position, velocity and
1269 * body-to-ECEF frame coordinate transformation.
1270 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
1271 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
1272 * frame, resolved along ECEF-frame axes.
1273 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
1274 * frame, resolved along ECEF-frame axes.
1275 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
1276 * frame, resolved along ECEF-frame axes.
1277 * @return a new body kinematics instance.
1278 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
1279 * are not ECEF frame valid.
1280 */
1281 public BodyKinematics estimateAndReturnNew(
1282 final double timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
1283 final Speed oldVx, final Speed oldVy, final Speed oldVz) {
1284 return estimateKinematicsAndReturnNew(timeInterval, frame, oldC, oldVx, oldVy, oldVz);
1285 }
1286
1287 /**
1288 * Estimates body kinematics (specific force applied to a body and its angular rates).
1289 *
1290 * @param timeInterval time interval between epochs.
1291 * @param frame body ECEF frame containing current position, velocity and
1292 * body-to-ECEF frame coordinate transformation.
1293 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
1294 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
1295 * frame, resolved along ECEF-frame axes.
1296 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
1297 * frame, resolved along ECEF-frame axes.
1298 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
1299 * frame, resolved along ECEF-frame axes.
1300 * @return a new body kinematics instance.
1301 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
1302 * are not ECEF frame valid.
1303 */
1304 public BodyKinematics estimateAndReturnNew(
1305 final Time timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
1306 final Speed oldVx, final Speed oldVy, final Speed oldVz) {
1307 return estimateKinematicsAndReturnNew(timeInterval, frame, oldC, oldVx, oldVy, oldVz);
1308 }
1309
1310 /**
1311 * Estimates body kinematics (specific force applied to a body and its angular rates).
1312 *
1313 * @param timeInterval time interval between epochs expressed in seconds (s).
1314 * @param c body-to-ECEF frame coordinate transformation matrix.
1315 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
1316 * @param vx velocity of body frame x coordinate with respect ECEF frame,
1317 * resolved along ECEF-frame axes and expressed in meters per
1318 * second (m/s).
1319 * @param vy velocity of body frame y coordinate with respect ECEF frame,
1320 * resolved along ECEF-frame axes and expressed in meters per
1321 * second (m/s).
1322 * @param vz velocity of body frame z coordinate with respect ECEF frame,
1323 * resolved along ECEF-frame axes and expressed in meters per
1324 * second (m/s).
1325 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
1326 * frame, resolved along ECEF-frame axes and expressed in meters
1327 * per second (m/s).
1328 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
1329 * frame, resolved along ECEF-frame axes and expressed in meters
1330 * per second (m/s).
1331 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
1332 * frame, resolved along ECEF-frame axes and expressed in meters
1333 * per second (m/s).
1334 * @param position body position expressed in meters (m) with respect ECEF frame,
1335 * resolved along ECEF-frame axes.
1336 * @return a new body kinematics instance.
1337 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
1338 * are not ECEF frame valid.
1339 */
1340 public BodyKinematics estimateAndReturnNew(
1341 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
1342 final double vx, final double vy, final double vz,
1343 final double oldVx, final double oldVy, final double oldVz, final Point3D position) {
1344 return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position);
1345 }
1346
1347 /**
1348 * Estimates body kinematics (specific force applied to a body and its angular rates).
1349 *
1350 * @param timeInterval time interval between epochs.
1351 * @param c body-to-ECEF frame coordinate transformation matrix.
1352 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
1353 * @param vx velocity of body frame x coordinate with respect ECEF frame,
1354 * resolved along ECEF-frame axes and expressed in meters per
1355 * second (m/s).
1356 * @param vy velocity of body frame y coordinate with respect ECEF frame,
1357 * resolved along ECEF-frame axes and expressed in meters per
1358 * second (m/s).
1359 * @param vz velocity of body frame z coordinate with respect ECEF frame,
1360 * resolved along ECEF-frame axes and expressed in meters per
1361 * second (m/s).
1362 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
1363 * frame, resolved along ECEF-frame axes and expressed in meters
1364 * per second (m/s).
1365 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
1366 * frame, resolved along ECEF-frame axes and expressed in meters
1367 * per second (m/s).
1368 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
1369 * frame, resolved along ECEF-frame axes and expressed in meters
1370 * per second (m/s).
1371 * @param position body position expressed in meters (m) with respect ECEF frame,
1372 * resolved along ECEF-frame axes.
1373 * @return a new body kinematics instance.
1374 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
1375 * are not ECEF frame valid.
1376 */
1377 public BodyKinematics estimateAndReturnNew(
1378 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
1379 final double vx, final double vy, final double vz,
1380 final double oldVx, final double oldVy, final double oldVz, final Point3D position) {
1381 return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position);
1382 }
1383
1384 /**
1385 * Estimates body kinematics (specific force applied to a body and its angular rates).
1386 *
1387 * @param timeInterval time interval between epochs expressed in seconds (s).
1388 * @param c body-to-ECEF frame coordinate transformation matrix.
1389 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
1390 * @param velocity velocity of body frame with respect ECEF frame.
1391 * @param oldVelocity previous velocity of body frame with respect ECEF frame.
1392 * @param position body position expressed in meters (m) with respect ECEF frame,
1393 * resolved along ECEF-frame axes.
1394 * @return a new body kinematics instance.
1395 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
1396 * are not ECEF frame valid.
1397 */
1398 public BodyKinematics estimateAndReturnNew(
1399 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
1400 final ECEFVelocity velocity, final ECEFVelocity oldVelocity, final Point3D position) {
1401 return estimateKinematicsAndReturnNew(timeInterval, c, oldC, velocity, oldVelocity, position);
1402 }
1403
1404 /**
1405 * Estimates body kinematics (specific force applied to a body and its angular rates).
1406 *
1407 * @param timeInterval time interval between epochs.
1408 * @param c body-to-ECEF frame coordinate transformation matrix.
1409 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
1410 * @param velocity velocity of body frame with respect ECEF frame.
1411 * @param oldVelocity previous velocity of body frame with respect ECEF frame.
1412 * @param position body position expressed in meters (m) with respect ECEF frame,
1413 * resolved along ECEF-frame axes.
1414 * @return a new body kinematics instance.
1415 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
1416 * are not ECEF frame valid.
1417 */
1418 public BodyKinematics estimateAndReturnNew(
1419 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
1420 final ECEFVelocity velocity, final ECEFVelocity oldVelocity, final Point3D position) {
1421 return estimateKinematicsAndReturnNew(timeInterval, c, oldC, velocity, oldVelocity, position);
1422 }
1423
1424 /**
1425 * Estimates body kinematics (specific force applied to a body and its angular rates).
1426 *
1427 * @param timeInterval time interval between epochs expressed in seconds (s).
1428 * @param c boy-to-ECEF frame coordinate transformation matrix.
1429 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
1430 * @param vx velocity of body frame x coordinate with respect ECEF frame,
1431 * resolved along ECEF-frame axes.
1432 * @param vy velocity of body frame y coordinate with respect ECEF frame,
1433 * resolved along ECEF-frame axes.
1434 * @param vz velocity of body frame z coordinate with respect ECEF frame,
1435 * resolved along ECEF-frame axes.
1436 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
1437 * frame, resolved along ECEF-frame axes.
1438 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
1439 * frame, resolved along ECEF-frame axes.
1440 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
1441 * frame, resolved along ECEF-frame axes.
1442 * @param position body position expressed in meters (m) with respect ECEF frame,
1443 * resolved along ECEF-frame axes.
1444 * @return a new body kinematics instance.
1445 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
1446 * are not ECEF frame valid.
1447 */
1448 public BodyKinematics estimateAndReturnNew(
1449 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
1450 final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
1451 final Point3D position) {
1452 return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position);
1453 }
1454
1455 /**
1456 * Estimates body kinematics (specific force applied to a body and its angular rates).
1457 *
1458 * @param timeInterval time interval between epochs.
1459 * @param c boy-to-ECEF frame coordinate transformation matrix.
1460 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
1461 * @param vx velocity of body frame x coordinate with respect ECEF frame,
1462 * resolved along ECEF-frame axes.
1463 * @param vy velocity of body frame y coordinate with respect ECEF frame,
1464 * resolved along ECEF-frame axes.
1465 * @param vz velocity of body frame z coordinate with respect ECEF frame,
1466 * resolved along ECEF-frame axes.
1467 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
1468 * frame, resolved along ECEF-frame axes.
1469 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
1470 * frame, resolved along ECEF-frame axes.
1471 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
1472 * frame, resolved along ECEF-frame axes.
1473 * @param position body position expressed in meters (m) with respect ECEF frame,
1474 * resolved along ECEF-frame axes.
1475 * @return a new body kinematics instance.
1476 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
1477 * are not ECEF frame valid.
1478 */
1479 public BodyKinematics estimateAndReturnNew(
1480 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
1481 final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
1482 final Point3D position) {
1483 return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position);
1484 }
1485
1486 /**
1487 * Estimates body kinematics (specific force applied to a body and its angular rates)..
1488 *
1489 * @param timeInterval time interval between epochs expressed in seconds (s).
1490 * @param c body-to-ECEF-frame coordinate transformation.
1491 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
1492 * @param vx x coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
1493 * along ECEF-frame axes.
1494 * @param vy y coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
1495 * along ECEF-frame axes.
1496 * @param vz z coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
1497 * along ECEF-frame axes.
1498 * @param oldVx x coordinate of previous velocity of body frame expressed in meters per second (m/s) and
1499 * resolved along ECEF-frame axes.
1500 * @param oldVy y coordinate of previous velocity of body frame expressed in meters per second (m/s) and
1501 * resolved along ECEF-frame axes.
1502 * @param oldVz z coordinate of previous velocity of body frame expressed in meters per second (m/s) and
1503 * resolved along ECEF-frame axes.
1504 * @param x cartesian x coordinate of body position expressed in meters (m) and resolved along
1505 * ECEF-frame axes.
1506 * @param y cartesian y coordinate of body position expressed in meters (m) and resolved along
1507 * ECEF-frame axes.
1508 * @param z cartesian z coordinate of body position expressed in meters (m) and resolved along
1509 * ECEF-frame axes.
1510 * @param result instance where estimated body kinematics will be stored.
1511 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
1512 * are not ECEF frame valid.
1513 */
1514 public static void estimateKinematics(
1515 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
1516 final double vx, final double vy, final double vz,
1517 final double oldVx, final double oldVy, final double oldVz,
1518 final double x, final double y, final double z, final BodyKinematics result) {
1519 if (timeInterval < 0.0 || !ECEFFrame.isValidCoordinateTransformation(c)
1520 || !ECEFFrame.isValidCoordinateTransformation(oldC)) {
1521 throw new IllegalArgumentException();
1522 }
1523
1524 if (timeInterval > 0.0) {
1525 try {
1526 // From (2.145) determine the Earth rotation over the update interval
1527 final var alpha = EARTH_ROTATION_RATE * timeInterval;
1528 final var cEarth = CoordinateTransformation.eciToEcefMatrixFromAngle(alpha);
1529
1530 final var cBe = c.getMatrix();
1531 cBe.transpose();
1532 final var oldCbe = oldC.getMatrix();
1533
1534 // cOldNew = cBe' * cEarth * oldCbe
1535 cEarth.multiply(oldCbe);
1536 // here cBe is cOldNew
1537 cBe.multiply(cEarth);
1538
1539 // Calculate the approximate angular rate with respect an inertial frame
1540 var alphaX = 0.5 * (cBe.getElementAt(1, 2) - cBe.getElementAt(2, 1));
1541 var alphaY = 0.5 * (cBe.getElementAt(2, 0) - cBe.getElementAt(0, 2));
1542 var alphaZ = 0.5 * (cBe.getElementAt(0, 1) - cBe.getElementAt(1, 0));
1543
1544 // Calculate and apply the scaling factor
1545 final var temp = Math.acos(0.5 * (Utils.trace(cBe) - 1.0));
1546 if (temp > SCALING_THRESHOLD) {
1547 // scaling is 1 if temp is less than this
1548 final var scale = temp / Math.sin(temp);
1549 alphaX *= scale;
1550 alphaY *= scale;
1551 alphaZ *= scale;
1552 }
1553
1554 // Calculate the angular rate
1555 final var angularRateX = alphaX / timeInterval;
1556 final var angularRateY = alphaY / timeInterval;
1557 final var angularRateZ = alphaZ / timeInterval;
1558
1559 // Calculate the specific force resolved about ECEF-frame axes
1560 // Frame (5.36)
1561 final var vEbe = new Matrix(ROWS, 1);
1562 vEbe.setElementAtIndex(0, vx);
1563 vEbe.setElementAtIndex(1, vy);
1564 vEbe.setElementAtIndex(2, vz);
1565
1566 final var oldVebe = new Matrix(ROWS, 1);
1567 oldVebe.setElementAtIndex(0, oldVx);
1568 oldVebe.setElementAtIndex(1, oldVy);
1569 oldVebe.setElementAtIndex(2, oldVz);
1570
1571 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(x, y, z);
1572 final var g = gravity.asMatrix();
1573
1574 final var earthRotationSkew = Utils.skewMatrix(new double[]{0.0, 0.0, EARTH_ROTATION_RATE});
1575 earthRotationSkew.multiplyByScalar(2.0);
1576 // 2.0 * earthRotationSkew * oldVebe
1577 earthRotationSkew.multiply(oldVebe);
1578
1579 // vEbe - oldVebe
1580 vEbe.subtract(oldVebe);
1581 // (vEbe - oldVebe) / timeInterval
1582 vEbe.multiplyByScalar(1.0 / timeInterval);
1583 // (vEbe - oldVebe) / timeInterval - g
1584 vEbe.subtract(g);
1585 // fibe = (vEbe - oldVebe) / timeInterval - g + 2.0 * earthRotationSkew * oldVebe
1586 // vEbe contains specific force resolved about ECEF-frame axes
1587 vEbe.add(earthRotationSkew);
1588
1589 // Calculate the average body-to-ECEF-frame coordinate transformation
1590 // matrix over the update interval using (5,84) and (5.85)
1591 final var alphaNorm = Math.sqrt(alphaX * alphaX + alphaY * alphaY + alphaZ * alphaZ);
1592 final var alphaSkew1 = Utils.skewMatrix(new double[]{alphaX, alphaY, alphaZ});
1593
1594 if (alphaNorm > ALPHA_THRESHOLD) {
1595 final var alphaNorm2 = alphaNorm * alphaNorm;
1596 final var value1 = (1.0 - Math.cos(alphaNorm)) / alphaNorm2;
1597 final var value2 = (1.0 - Math.sin(alphaNorm) / alphaNorm) / alphaNorm2;
1598 final var tmp1 = alphaSkew1.multiplyByScalarAndReturnNew(value1);
1599 final var tmp2 = alphaSkew1.multiplyByScalarAndReturnNew(value2);
1600 tmp2.multiply(alphaSkew1);
1601
1602 final var tmp3 = Matrix.identity(ROWS, ROWS);
1603 tmp3.add(tmp1);
1604 tmp3.add(tmp2);
1605
1606 oldCbe.multiply(tmp3);
1607 }
1608
1609 final var alphaSkew2 = Utils.skewMatrix(new double[]{0.0, 0.0, alpha});
1610 alphaSkew2.multiplyByScalar(0.5);
1611 // 0.5 * alphaSkew2 * oldCbe
1612 alphaSkew2.multiply(oldCbe);
1613
1614 // oldCbe - 0.5 * alphaSkew2 * oldCbe
1615 oldCbe.subtract(alphaSkew2);
1616 // oldCbe now contains the average body-to-ECEF-frame coordinate transformation
1617
1618 // Transform specific force to body-frame resolving axes using (5.81)
1619 final var invAveCbe = Utils.inverse(oldCbe);
1620 invAveCbe.multiply(vEbe);
1621
1622 final var specificForceX = invAveCbe.getElementAtIndex(0);
1623 final var specificForceY = invAveCbe.getElementAtIndex(1);
1624 final var specificForceZ = invAveCbe.getElementAtIndex(2);
1625
1626 // save result data
1627 result.setSpecificForceCoordinates(specificForceX, specificForceY, specificForceZ);
1628 result.setAngularRateCoordinates(angularRateX, angularRateY, angularRateZ);
1629
1630 } catch (final AlgebraException ignore) {
1631 // never happens
1632 }
1633 } else {
1634 // If time interval is zero, set angular rate and specific force to zero
1635 result.setSpecificForceCoordinates(0.0, 0.0, 0.0);
1636 result.setAngularRateCoordinates(0.0, 0.0, 0.0);
1637 }
1638 }
1639
1640 /**
1641 * Estimates body kinematics (specific force applied to a body and its angular rates).
1642 *
1643 * @param timeInterval time interval between epochs.
1644 * @param c boy-to-ECEF frame coordinate transformation matrix.
1645 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
1646 * @param vx velocity of body frame x coordinate with respect ECEF frame,
1647 * resolved along ECEF-frame axes and expressed in meters per
1648 * second (m/s).
1649 * @param vy velocity of body frame y coordinate with respect ECEF frame,
1650 * resolved along ECEF-frame axes and expressed in meters per
1651 * second (m/s).
1652 * @param vz velocity of body frame z coordinate with respect ECEF frame,
1653 * resolved along ECEF-frame axes and expressed in meters per
1654 * second (m/s).
1655 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
1656 * frame, resolved along ECEF-frame axes and expressed in meters
1657 * per second (m/s).
1658 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
1659 * frame, resolved along ECEF-frame axes and expressed in meters
1660 * per second (m/s).
1661 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
1662 * frame, resolved along ECEF-frame axes and expressed in meters
1663 * per second (m/s).
1664 * @param x cartesian x coordinate of body position expressed in meters (m)
1665 * with respect ECEF frame, resolved along ECEF-frame axes.
1666 * @param y cartesian y coordinate of body position expressed in meters (m)
1667 * with respect ECEF frame, resolved along ECEF-frame axes.
1668 * @param z cartesian z coordinate of body position expressed in meters (m)
1669 * with respect ECEF frame, resolved along ECEF-frame axes.
1670 * @param result instance where body kinematics estimation will be stored.
1671 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
1672 * are not ECEF frame valid.
1673 */
1674 public static void estimateKinematics(
1675 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
1676 final double vx, final double vy, final double vz,
1677 final double oldVx, final double oldVy, final double oldVz,
1678 final double x, final double y, final double z, final BodyKinematics result) {
1679 estimateKinematics(
1680 TimeConverter.convert(timeInterval.getValue().doubleValue(), timeInterval.getUnit(), TimeUnit.SECOND),
1681 c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z, result);
1682 }
1683
1684 /**
1685 * Estimates body kinematics (specific force applied to a body and its angular rates).
1686 *
1687 * @param timeInterval time interval between epochs expressed in seconds (s).
1688 * @param c boy-to-ECEF frame coordinate transformation matrix.
1689 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
1690 * @param velocity velocity of body frame with respect ECEF frame.
1691 * @param oldVelocity previous velocity of body frame with respect ECEF frame.
1692 * @param x cartesian x coordinate of body position expressed in meters (m)
1693 * with respect ECEF frame, resolved along ECEF-frame axes.
1694 * @param y cartesian y coordinate of body position expressed in meters (m)
1695 * with respect ECEF frame, resolved along ECEF-frame axes.
1696 * @param z cartesian z coordinate of body position expressed in meters (m)
1697 * with respect ECEF frame, resolved along ECEF-frame axes.
1698 * @param result instance where body kinematics estimation will be stored.
1699 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
1700 * are not ECEF frame valid.
1701 */
1702 public static void estimateKinematics(
1703 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
1704 final ECEFVelocity velocity, final ECEFVelocity oldVelocity, final double x, final double y, final double z,
1705 final BodyKinematics result) {
1706 estimateKinematics(timeInterval, c, oldC, velocity.getVx(), velocity.getVy(), velocity.getVz(),
1707 oldVelocity.getVx(), oldVelocity.getVy(), oldVelocity.getVz(), x, y, z, result);
1708 }
1709
1710 /**
1711 * Estimates body kinematics (specific force applied to a body and its angular rates).
1712 *
1713 * @param timeInterval time interval between epochs.
1714 * @param c boy-to-ECEF frame coordinate transformation matrix.
1715 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
1716 * @param velocity velocity of body frame with respect ECEF frame.
1717 * @param oldVelocity previous velocity of body frame with respect ECEF frame.
1718 * @param x cartesian x coordinate of body position expressed in meters (m)
1719 * with respect ECEF frame, resolved along ECEF-frame axes.
1720 * @param y cartesian y coordinate of body position expressed in meters (m)
1721 * with respect ECEF frame, resolved along ECEF-frame axes.
1722 * @param z cartesian z coordinate of body position expressed in meters (m)
1723 * with respect ECEF frame, resolved along ECEF-frame axes.
1724 * @param result instance where body kinematics estimation will be stored.
1725 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
1726 * are not ECEF frame valid.
1727 */
1728 public static void estimateKinematics(
1729 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
1730 final ECEFVelocity velocity, final ECEFVelocity oldVelocity, final double x, final double y, final double z,
1731 final BodyKinematics result) {
1732 estimateKinematics(
1733 TimeConverter.convert(timeInterval.getValue().doubleValue(), timeInterval.getUnit(), TimeUnit.SECOND),
1734 c, oldC, velocity, oldVelocity, x, y, z, result);
1735 }
1736
1737 /**
1738 * Estimates body kinematics (specific force applied to a body and its angular rates)..
1739 *
1740 * @param timeInterval time interval between epochs expressed in seconds (s).
1741 * @param c body-to-ECEF-frame coordinate transformation.
1742 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
1743 * @param vx x coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
1744 * along ECEF-frame axes.
1745 * @param vy y coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
1746 * along ECEF-frame axes.
1747 * @param vz z coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
1748 * along ECEF-frame axes.
1749 * @param oldVx x coordinate of previous velocity of body frame expressed in meters per second (m/s) and
1750 * resolved along ECEF-frame axes.
1751 * @param oldVy y coordinate of previous velocity of body frame expressed in meters per second (m/s) and
1752 * resolved along ECEF-frame axes.
1753 * @param oldVz z coordinate of previous velocity of body frame expressed in meters per second (m/s) and
1754 * resolved along ECEF-frame axes.
1755 * @param position cartesian body position resolved along ECEF-frame axes.
1756 * @param result instance where estimated body kinematics will be stored.
1757 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
1758 * are not ECEF frame valid.
1759 */
1760 public static void estimateKinematics(
1761 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
1762 final double vx, final double vy, final double vz,
1763 final double oldVx, final double oldVy, final double oldVz, final ECEFPosition position,
1764 final BodyKinematics result) {
1765 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz,
1766 position.getX(), position.getY(), position.getZ(), result);
1767 }
1768
1769 /**
1770 * Estimates body kinematics (specific force applied to a body and its angular rates).
1771 *
1772 * @param timeInterval time interval between epochs.
1773 * @param c body-to-ECEF-frame coordinate transformation.
1774 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
1775 * @param vx x coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
1776 * along ECEF-frame axes.
1777 * @param vy y coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
1778 * along ECEF-frame axes.
1779 * @param vz z coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
1780 * along ECEF-frame axes.
1781 * @param oldVx x coordinate of previous velocity of body frame expressed in meters per second (m/s) and
1782 * resolved along ECEF-frame axes.
1783 * @param oldVy y coordinate of previous velocity of body frame expressed in meters per second (m/s) and
1784 * resolved along ECEF-frame axes.
1785 * @param oldVz z coordinate of previous velocity of body frame expressed in meters per second (m/s) and
1786 * resolved along ECEF-frame axes.
1787 * @param position cartesian body position resolved along ECEF-frame axes.
1788 * @param result instance where estimated body kinematics will be stored.
1789 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
1790 * are not ECEF frame valid.
1791 */
1792 public static void estimateKinematics(
1793 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
1794 final double vx, final double vy, final double vz,
1795 final double oldVx, final double oldVy, final double oldVz, final ECEFPosition position,
1796 final BodyKinematics result) {
1797 estimateKinematics(
1798 TimeConverter.convert(timeInterval.getValue().doubleValue(), timeInterval.getUnit(), TimeUnit.SECOND),
1799 c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position, result);
1800 }
1801
1802 /**
1803 * Estimates body kinematics (specific force applied to a body and its angular rates).
1804 *
1805 * @param timeInterval time interval between epochs expressed in seconds (s).
1806 * @param c boy-to-ECEF frame coordinate transformation matrix.
1807 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
1808 * @param velocity velocity of body frame with respect ECEF frame.
1809 * @param oldVelocity previous velocity of body frame with respect ECEF frame.
1810 * @param position cartesian body position resolved along ECEF-frame axes.
1811 * @param result instance where body kinematics estimation will be stored.
1812 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
1813 * are not ECEF frame valid.
1814 */
1815 public static void estimateKinematics(
1816 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
1817 final ECEFVelocity velocity, final ECEFVelocity oldVelocity, final ECEFPosition position,
1818 final BodyKinematics result) {
1819 estimateKinematics(timeInterval, c, oldC, velocity.getVx(), velocity.getVy(), velocity.getVz(),
1820 oldVelocity.getVx(), oldVelocity.getVy(), oldVelocity.getVz(),
1821 position.getX(), position.getY(), position.getZ(), result);
1822 }
1823
1824 /**
1825 * Estimates body kinematics (specific force applied to a body and its angular rates).
1826 *
1827 * @param timeInterval time interval between epochs.
1828 * @param c boy-to-ECEF frame coordinate transformation matrix.
1829 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
1830 * @param velocity velocity of body frame with respect ECEF frame.
1831 * @param oldVelocity previous velocity of body frame with respect ECEF frame.
1832 * @param position cartesian body position resolved along ECEF-frame axes.
1833 * @param result instance where body kinematics estimation will be stored.
1834 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
1835 * are not ECEF frame valid.
1836 */
1837 public static void estimateKinematics(
1838 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
1839 final ECEFVelocity velocity, final ECEFVelocity oldVelocity, final ECEFPosition position,
1840 final BodyKinematics result) {
1841 estimateKinematics(
1842 TimeConverter.convert(timeInterval.getValue().doubleValue(), timeInterval.getUnit(), TimeUnit.SECOND),
1843 c, oldC, velocity, oldVelocity, position, result);
1844 }
1845
1846 /**
1847 * Estimates body kinematics (specific force applied to a body and its angular rates).
1848 *
1849 * @param timeInterval time interval between epochs expressed in seconds (s).
1850 * @param frame body ECEF frame containing current position, velocity and
1851 * body-to-ECEF frame coordinate transformation.
1852 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
1853 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
1854 * frame, resolved along ECEF-frame axes and expressed in meters
1855 * per second (m/s).
1856 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
1857 * frame, resolved along ECEF-frame axes and expressed in meters
1858 * per second (m/s).
1859 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
1860 * frame, resolved along ECEF-frame axes and expressed in meters
1861 * per second (m/s).
1862 * @param result instance where body kinematics estimation will be stored.
1863 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
1864 * are not ECEF frame valid.
1865 */
1866 public static void estimateKinematics(
1867 final double timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
1868 final double oldVx, final double oldVy, final double oldVz, final BodyKinematics result) {
1869 estimateKinematics(timeInterval, frame.getCoordinateTransformation(), oldC,
1870 frame.getVx(), frame.getVy(), frame.getVz(), oldVx, oldVy, oldVz,
1871 frame.getX(), frame.getY(), frame.getZ(), result);
1872 }
1873
1874 /**
1875 * Estimates body kinematics (specific force applied to a body and its angular rates).
1876 *
1877 * @param timeInterval time interval between epochs.
1878 * @param frame body ECEF frame containing current position, velocity and
1879 * body-to-ECEF frame coordinate transformation.
1880 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
1881 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
1882 * frame, resolved along ECEF-frame axes and expressed in meters
1883 * per second (m/s).
1884 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
1885 * frame, resolved along ECEF-frame axes and expressed in meters
1886 * per second (m/s).
1887 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
1888 * frame, resolved along ECEF-frame axes and expressed in meters
1889 * per second (m/s).
1890 * @param result instance where body kinematics estimation will be stored.
1891 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
1892 * are not ECEF frame valid.
1893 */
1894 public static void estimateKinematics(
1895 final Time timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
1896 final double oldVx, final double oldVy, final double oldVz, final BodyKinematics result) {
1897 estimateKinematics(
1898 TimeConverter.convert(timeInterval.getValue().doubleValue(), timeInterval.getUnit(), TimeUnit.SECOND),
1899 frame.getCoordinateTransformation(), oldC, frame.getVx(), frame.getVy(), frame.getVz(),
1900 oldVx, oldVy, oldVz, frame.getX(), frame.getY(), frame.getZ(), result);
1901 }
1902
1903 /**
1904 * Estimates body kinematics (specific force applied to a body and its angular rates).
1905 *
1906 * @param timeInterval time interval between epochs expressed in seconds (s).
1907 * @param frame body ECEF frame containing current position, velocity and
1908 * body-to-ECEF frame coordinate transformation.
1909 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
1910 * @param oldVelocity previous velocity of body frame with respect ECEF frame.
1911 * @param result instance where body kinematics estimation will be stored.
1912 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
1913 * are not ECEF frame valid.
1914 */
1915 public static void estimateKinematics(
1916 final double timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
1917 final ECEFVelocity oldVelocity, final BodyKinematics result) {
1918 estimateKinematics(timeInterval, frame, oldC, oldVelocity.getVx(), oldVelocity.getVy(), oldVelocity.getVz(),
1919 result);
1920 }
1921
1922 /**
1923 * Estimates body kinematics (specific force applied to a body and its angular rates).
1924 *
1925 * @param timeInterval time interval between epochs.
1926 * @param frame body ECEF frame containing current position, velocity and
1927 * body-to-ECEF frame coordinate transformation.
1928 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
1929 * @param oldVelocity previous velocity of body frame with respect ECEF frame.
1930 * @param result instance where body kinematics estimation will be stored.
1931 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
1932 * are not ECEF frame valid.
1933 */
1934 public static void estimateKinematics(
1935 final Time timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
1936 final ECEFVelocity oldVelocity, final BodyKinematics result) {
1937 estimateKinematics(
1938 TimeConverter.convert(timeInterval.getValue().doubleValue(), timeInterval.getUnit(), TimeUnit.SECOND),
1939 frame, oldC, oldVelocity, result);
1940 }
1941
1942 /**
1943 * Estimates body kinematics (specific force applied to a body and its angular rates).
1944 *
1945 * @param timeInterval time interval between epochs expressed in seconds (s).
1946 * @param frame body ECEF frame containing current position, velocity and
1947 * body-to-ECEF frame coordinate transformation.
1948 * @param oldFrame body ECEF frame containing previous position, velocity and
1949 * body-to-ECEF frame coordinate transformation. Notice that
1950 * previous position contained in this frame is ignored.
1951 * @param result instance where body kinematics estimation will be stored.
1952 * @throws IllegalArgumentException if provided time interval is negative.
1953 */
1954 public static void estimateKinematics(
1955 final double timeInterval, final ECEFFrame frame, final ECEFFrame oldFrame, final BodyKinematics result) {
1956 estimateKinematics(timeInterval, frame, oldFrame.getCoordinateTransformation(),
1957 oldFrame.getVx(), oldFrame.getVy(), oldFrame.getVz(), result);
1958 }
1959
1960 /**
1961 * Estimates body kinematics (specific force applied to a body and its angular rates).
1962 *
1963 * @param timeInterval time interval between epochs.
1964 * @param frame body ECEF frame containing current position, velocity and
1965 * body-to-ECEF frame coordinate transformation.
1966 * @param oldFrame body ECEF frame containing previous position, velocity and
1967 * body-to-ECEF frame coordinate transformation. Notice that
1968 * previous position contained in this frame is ignored.
1969 * @param result instance where body kinematics estimation will be stored.
1970 * @throws IllegalArgumentException if provided time interval is negative.
1971 */
1972 public static void estimateKinematics(
1973 final Time timeInterval, final ECEFFrame frame, final ECEFFrame oldFrame, final BodyKinematics result) {
1974 estimateKinematics(timeInterval, frame, oldFrame.getCoordinateTransformation(),
1975 oldFrame.getVx(), oldFrame.getVy(), oldFrame.getVz(), result);
1976 }
1977
1978 /**
1979 * Estimates body kinematics (specific force applied to a body and its angular rates).
1980 *
1981 * @param timeInterval time interval between epochs expressed in seconds (s).
1982 * @param c body-to-ECEF frame coordinate transformation matrix.
1983 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
1984 * @param vx velocity of body frame x coordinate with respect ECEF frame,
1985 * resolved along ECEF-frame axes.
1986 * @param vy velocity of body frame y coordinate with respect ECEF frame,
1987 * resolved along ECEF-frame axes.
1988 * @param vz velocity of body frame z coordinate with respect ECEF frame,
1989 * resolved along ECEF-frame axes.
1990 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
1991 * frame, resolved along ECEF-frame axes.
1992 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
1993 * frame, resolved along ECEF-frame axes.
1994 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
1995 * frame, resolved along ECEF-frame axes.
1996 * @param x cartesian x coordinate of body position expressed in meters (m)
1997 * with respect ECEF frame, resolved along ECEF-frame axes.
1998 * @param y cartesian y coordinate of body position expressed in meters (m)
1999 * with respect ECEF frame, resolved along ECEF-frame axes.
2000 * @param z cartesian z coordinate of body position expressed in meters (m)
2001 * with respect ECEF frame, resolved along ECEF-frame axes.
2002 * @param result instance where body kinematics estimation will be stored.
2003 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
2004 * are not ECEF frame valid.
2005 */
2006 public static void estimateKinematics(
2007 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
2008 final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
2009 final double x, final double y, final double z, final BodyKinematics result) {
2010 estimateKinematics(timeInterval, c, oldC,
2011 SpeedConverter.convert(vx.getValue().doubleValue(), vx.getUnit(), SpeedUnit.METERS_PER_SECOND),
2012 SpeedConverter.convert(vy.getValue().doubleValue(), vy.getUnit(), SpeedUnit.METERS_PER_SECOND),
2013 SpeedConverter.convert(vz.getValue().doubleValue(), vz.getUnit(), SpeedUnit.METERS_PER_SECOND),
2014 SpeedConverter.convert(oldVx.getValue().doubleValue(), oldVx.getUnit(), SpeedUnit.METERS_PER_SECOND),
2015 SpeedConverter.convert(oldVy.getValue().doubleValue(), oldVy.getUnit(), SpeedUnit.METERS_PER_SECOND),
2016 SpeedConverter.convert(oldVz.getValue().doubleValue(), oldVz.getUnit(), SpeedUnit.METERS_PER_SECOND),
2017 x, y, z, result);
2018 }
2019
2020 /**
2021 * Estimates body kinematics (specific force applied to a body and its angular rates).
2022 *
2023 * @param timeInterval time interval between epochs.
2024 * @param c body-to-ECEF frame coordinate transformation matrix.
2025 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
2026 * @param vx velocity of body frame x coordinate with respect ECEF frame,
2027 * resolved along ECEF-frame axes.
2028 * @param vy velocity of body frame y coordinate with respect ECEF frame,
2029 * resolved along ECEF-frame axes.
2030 * @param vz velocity of body frame z coordinate with respect ECEF frame,
2031 * resolved along ECEF-frame axes.
2032 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
2033 * frame, resolved along ECEF-frame axes.
2034 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
2035 * frame, resolved along ECEF-frame axes.
2036 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
2037 * frame, resolved along ECEF-frame axes.
2038 * @param x cartesian x coordinate of body position expressed in meters (m)
2039 * with respect ECEF frame, resolved along ECEF-frame axes.
2040 * @param y cartesian y coordinate of body position expressed in meters (m)
2041 * with respect ECEF frame, resolved along ECEF-frame axes.
2042 * @param z cartesian z coordinate of body position expressed in meters (m)
2043 * with respect ECEF frame, resolved along ECEF-frame axes.
2044 * @param result instance where body kinematics estimation will be stored.
2045 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
2046 * are not ECEF frame valid.
2047 */
2048 public static void estimateKinematics(
2049 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
2050 final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
2051 final double x, final double y, final double z, final BodyKinematics result) {
2052 estimateKinematics(timeInterval, c, oldC,
2053 SpeedConverter.convert(vx.getValue().doubleValue(), vx.getUnit(), SpeedUnit.METERS_PER_SECOND),
2054 SpeedConverter.convert(vy.getValue().doubleValue(), vy.getUnit(), SpeedUnit.METERS_PER_SECOND),
2055 SpeedConverter.convert(vz.getValue().doubleValue(), vz.getUnit(), SpeedUnit.METERS_PER_SECOND),
2056 SpeedConverter.convert(oldVx.getValue().doubleValue(), oldVx.getUnit(), SpeedUnit.METERS_PER_SECOND),
2057 SpeedConverter.convert(oldVy.getValue().doubleValue(), oldVy.getUnit(), SpeedUnit.METERS_PER_SECOND),
2058 SpeedConverter.convert(oldVz.getValue().doubleValue(), oldVz.getUnit(), SpeedUnit.METERS_PER_SECOND),
2059 x, y, z, result);
2060 }
2061
2062 /**
2063 * Estimates body kinematics (specific force applied to a body and its angular rates).
2064 *
2065 * @param timeInterval time interval between epochs expressed in seconds (s).
2066 * @param c body-to-ECEF frame coordinate transformation matrix.
2067 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
2068 * @param vx velocity of body frame x coordinate with respect ECEF frame,
2069 * resolved along ECEF-frame axes.
2070 * @param vy velocity of body frame y coordinate with respect ECEF frame,
2071 * resolved along ECEF-frame axes.
2072 * @param vz velocity of body frame z coordinate with respect ECEF frame,
2073 * resolved along ECEF-frame axes.
2074 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
2075 * frame, resolved along ECEF-frame axes.
2076 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
2077 * frame, resolved along ECEF-frame axes.
2078 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
2079 * frame, resolved along ECEF-frame axes.
2080 * @param position cartesian body position resolved along ECEF-frame axes.
2081 * @param result instance where body kinematics estimation will be stored.
2082 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
2083 * are not ECEF frame valid.
2084 */
2085 public static void estimateKinematics(
2086 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
2087 final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
2088 final ECEFPosition position, final BodyKinematics result) {
2089 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz,
2090 position.getX(), position.getY(), position.getZ(), result);
2091 }
2092
2093 /**
2094 * Estimates body kinematics (specific force applied to a body and its angular rates).
2095 *
2096 * @param timeInterval time interval between epochs.
2097 * @param c body-to-ECEF frame coordinate transformation matrix.
2098 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
2099 * @param vx velocity of body frame x coordinate with respect ECEF frame,
2100 * resolved along ECEF-frame axes.
2101 * @param vy velocity of body frame y coordinate with respect ECEF frame,
2102 * resolved along ECEF-frame axes.
2103 * @param vz velocity of body frame z coordinate with respect ECEF frame,
2104 * resolved along ECEF-frame axes.
2105 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
2106 * frame, resolved along ECEF-frame axes.
2107 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
2108 * frame, resolved along ECEF-frame axes.
2109 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
2110 * frame, resolved along ECEF-frame axes.
2111 * @param position cartesian body position resolved along ECEF-frame axes.
2112 * @param result instance where body kinematics estimation will be stored.
2113 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
2114 * are not ECEF frame valid.
2115 */
2116 public static void estimateKinematics(
2117 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
2118 final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
2119 final ECEFPosition position, final BodyKinematics result) {
2120 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz,
2121 position.getX(), position.getY(), position.getZ(), result);
2122 }
2123
2124 /**
2125 * Estimates body kinematics (specific force applied to a body and its angular rates).
2126 *
2127 * @param timeInterval time interval between epochs expressed in seconds (s).
2128 * @param frame body ECEF frame containing current position, velocity and
2129 * body-to-ECEF frame coordinate transformation.
2130 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
2131 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
2132 * frame, resolved along ECEF-frame axes.
2133 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
2134 * frame, resolved along ECEF-frame axes.
2135 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
2136 * frame, resolved along ECEF-frame axes.
2137 * @param result instance where body kinematics estimation will be stored.
2138 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
2139 * are not ECEF frame valid.
2140 */
2141 public static void estimateKinematics(
2142 final double timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
2143 final Speed oldVx, final Speed oldVy, final Speed oldVz, final BodyKinematics result) {
2144 estimateKinematics(timeInterval, frame, oldC,
2145 SpeedConverter.convert(oldVx.getValue().doubleValue(), oldVx.getUnit(), SpeedUnit.METERS_PER_SECOND),
2146 SpeedConverter.convert(oldVy.getValue().doubleValue(), oldVy.getUnit(), SpeedUnit.METERS_PER_SECOND),
2147 SpeedConverter.convert(oldVz.getValue().doubleValue(), oldVz.getUnit(), SpeedUnit.METERS_PER_SECOND),
2148 result);
2149 }
2150
2151 /**
2152 * Estimates body kinematics (specific force applied to a body and its angular rates).
2153 *
2154 * @param timeInterval time interval between epochs.
2155 * @param frame body ECEF frame containing current position, velocity and
2156 * body-to-ECEF frame coordinate transformation.
2157 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
2158 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
2159 * frame, resolved along ECEF-frame axes.
2160 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
2161 * frame, resolved along ECEF-frame axes.
2162 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
2163 * frame, resolved along ECEF-frame axes.
2164 * @param result instance where body kinematics estimation will be stored.
2165 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
2166 * are not ECEF frame valid.
2167 */
2168 public static void estimateKinematics(
2169 final Time timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
2170 final Speed oldVx, final Speed oldVy, final Speed oldVz, final BodyKinematics result) {
2171 estimateKinematics(timeInterval, frame, oldC,
2172 SpeedConverter.convert(oldVx.getValue().doubleValue(), oldVx.getUnit(), SpeedUnit.METERS_PER_SECOND),
2173 SpeedConverter.convert(oldVy.getValue().doubleValue(), oldVy.getUnit(), SpeedUnit.METERS_PER_SECOND),
2174 SpeedConverter.convert(oldVz.getValue().doubleValue(), oldVz.getUnit(), SpeedUnit.METERS_PER_SECOND),
2175 result);
2176 }
2177
2178 /**
2179 * Estimates body kinematics (specific force applied to a body and its angular rates).
2180 *
2181 * @param timeInterval time interval between epochs expressed in seconds (s).
2182 * @param c body-to-ECEF frame coordinate transformation matrix.
2183 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
2184 * @param vx velocity of body frame x coordinate with respect ECEF frame,
2185 * resolved along ECEF-frame axes and expressed in meters per
2186 * second (m/s).
2187 * @param vy velocity of body frame y coordinate with respect ECEF frame,
2188 * resolved along ECEF-frame axes and expressed in meters per
2189 * second (m/s).
2190 * @param vz velocity of body frame z coordinate with respect ECEF frame,
2191 * resolved along ECEF-frame axes and expressed in meters per
2192 * second (m/s).
2193 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
2194 * frame, resolved along ECEF-frame axes and expressed in meters
2195 * per second (m/s).
2196 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
2197 * frame, resolved along ECEF-frame axes and expressed in meters
2198 * per second (m/s).
2199 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
2200 * frame, resolved along ECEF-frame axes and expressed in meters
2201 * per second (m/s).
2202 * @param position body position expressed in meters (m) with respect ECEF frame,
2203 * resolved along ECEF-frame axes.
2204 * @param result instance where body kinematics estimation will be stored.
2205 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
2206 * are not ECEF frame valid.
2207 */
2208 public static void estimateKinematics(
2209 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
2210 final double vx, final double vy, final double vz,
2211 final double oldVx, final double oldVy, final double oldVz, final Point3D position,
2212 final BodyKinematics result) {
2213 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz,
2214 position.getInhomX(), position.getInhomY(), position.getInhomZ(), result);
2215 }
2216
2217 /**
2218 * Estimates body kinematics (specific force applied to a body and its angular rates).
2219 *
2220 * @param timeInterval time interval between epochs.
2221 * @param c body-to-ECEF frame coordinate transformation matrix.
2222 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
2223 * @param vx velocity of body frame x coordinate with respect ECEF frame,
2224 * resolved along ECEF-frame axes and expressed in meters per
2225 * second (m/s).
2226 * @param vy velocity of body frame y coordinate with respect ECEF frame,
2227 * resolved along ECEF-frame axes and expressed in meters per
2228 * second (m/s).
2229 * @param vz velocity of body frame z coordinate with respect ECEF frame,
2230 * resolved along ECEF-frame axes and expressed in meters per
2231 * second (m/s).
2232 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
2233 * frame, resolved along ECEF-frame axes and expressed in meters
2234 * per second (m/s).
2235 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
2236 * frame, resolved along ECEF-frame axes and expressed in meters
2237 * per second (m/s).
2238 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
2239 * frame, resolved along ECEF-frame axes and expressed in meters
2240 * per second (m/s).
2241 * @param position body position expressed in meters (m) with respect ECEF frame,
2242 * resolved along ECEF-frame axes.
2243 * @param result instance where body kinematics estimation will be stored.
2244 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
2245 * are not ECEF frame valid.
2246 */
2247 public static void estimateKinematics(
2248 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
2249 final double vx, final double vy, final double vz,
2250 final double oldVx, final double oldVy, final double oldVz, final Point3D position,
2251 final BodyKinematics result) {
2252 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz,
2253 position.getInhomX(), position.getInhomY(), position.getInhomZ(), result);
2254 }
2255
2256 /**
2257 * Estimates body kinematics (specific force applied to a body and its angular rates).
2258 *
2259 * @param timeInterval time interval between epochs expressed in seconds (s).
2260 * @param c body-to-ECEF frame coordinate transformation matrix.
2261 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
2262 * @param velocity velocity of body frame with respect ECEF frame.
2263 * @param oldVelocity previous velocity of body frame with respect ECEF frame.
2264 * @param position body position expressed in meters (m) with respect ECEF frame,
2265 * resolved along ECEF-frame axes.
2266 * @param result instance where body kinematics estimation will be stored.
2267 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
2268 * are not ECEF frame valid.
2269 */
2270 public static void estimateKinematics(
2271 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
2272 final ECEFVelocity velocity, final ECEFVelocity oldVelocity, final Point3D position,
2273 final BodyKinematics result) {
2274 estimateKinematics(timeInterval, c, oldC, velocity, oldVelocity,
2275 position.getInhomX(), position.getInhomY(), position.getInhomZ(), result);
2276 }
2277
2278 /**
2279 * Estimates body kinematics (specific force applied to a body and its angular rates).
2280 *
2281 * @param timeInterval time interval between epochs.
2282 * @param c body-to-ECEF frame coordinate transformation matrix.
2283 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
2284 * @param velocity velocity of body frame with respect ECEF frame.
2285 * @param oldVelocity previous velocity of body frame with respect ECEF frame.
2286 * @param position body position expressed in meters (m) with respect ECEF frame,
2287 * resolved along ECEF-frame axes.
2288 * @param result instance where body kinematics estimation will be stored.
2289 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
2290 * are not ECEF frame valid.
2291 */
2292 public static void estimateKinematics(
2293 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
2294 final ECEFVelocity velocity, final ECEFVelocity oldVelocity, final Point3D position,
2295 final BodyKinematics result) {
2296 estimateKinematics(timeInterval, c, oldC, velocity, oldVelocity,
2297 position.getInhomX(), position.getInhomY(), position.getInhomZ(), result);
2298 }
2299
2300 /**
2301 * Estimates body kinematics (specific force applied to a body and its angular rates).
2302 *
2303 * @param timeInterval time interval between epochs expressed in seconds (s).
2304 * @param c boy-to-ECEF frame coordinate transformation matrix.
2305 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
2306 * @param vx velocity of body frame x coordinate with respect ECEF frame,
2307 * resolved along ECEF-frame axes.
2308 * @param vy velocity of body frame y coordinate with respect ECEF frame,
2309 * resolved along ECEF-frame axes.
2310 * @param vz velocity of body frame z coordinate with respect ECEF frame,
2311 * resolved along ECEF-frame axes.
2312 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
2313 * frame, resolved along ECEF-frame axes.
2314 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
2315 * frame, resolved along ECEF-frame axes.
2316 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
2317 * frame, resolved along ECEF-frame axes.
2318 * @param position body position expressed in meters (m) with respect ECEF frame,
2319 * resolved along ECEF-frame axes.
2320 * @param result instance where body kinematics estimation will be stored.
2321 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
2322 * are not ECEF frame valid.
2323 */
2324 public static void estimateKinematics(
2325 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
2326 final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
2327 final Point3D position, final BodyKinematics result) {
2328 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz,
2329 position.getInhomX(), position.getInhomY(), position.getInhomZ(), result);
2330 }
2331
2332 /**
2333 * Estimates body kinematics (specific force applied to a body and its angular rates).
2334 *
2335 * @param timeInterval time interval between epochs.
2336 * @param c boy-to-ECEF frame coordinate transformation matrix.
2337 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
2338 * @param vx velocity of body frame x coordinate with respect ECEF frame,
2339 * resolved along ECEF-frame axes.
2340 * @param vy velocity of body frame y coordinate with respect ECEF frame,
2341 * resolved along ECEF-frame axes.
2342 * @param vz velocity of body frame z coordinate with respect ECEF frame,
2343 * resolved along ECEF-frame axes.
2344 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
2345 * frame, resolved along ECEF-frame axes.
2346 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
2347 * frame, resolved along ECEF-frame axes.
2348 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
2349 * frame, resolved along ECEF-frame axes.
2350 * @param position body position expressed in meters (m) with respect ECEF frame,
2351 * resolved along ECEF-frame axes.
2352 * @param result instance where body kinematics estimation will be stored.
2353 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
2354 * are not ECEF frame valid.
2355 */
2356 public static void estimateKinematics(
2357 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
2358 final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
2359 final Point3D position, final BodyKinematics result) {
2360 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz,
2361 position.getInhomX(), position.getInhomY(), position.getInhomZ(), result);
2362 }
2363
2364 /**
2365 * Estimates body kinematics (specific force applied to a body and its angular rates).
2366 *
2367 * @param timeInterval time interval between epochs expressed in seconds (s).
2368 * @param c body-to-ECEF frame coordinate transformation matrix.
2369 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
2370 * @param vx velocity of body frame x coordinate with respect ECEF frame,
2371 * resolved along ECEF-frame axes and expressed in meters per
2372 * second (m/s).
2373 * @param vy velocity of body frame y coordinate with respect ECEF frame,
2374 * resolved along ECEF-frame axes and expressed in meters per
2375 * second (m/s).
2376 * @param vz velocity of body frame z coordinate with respect ECEF frame,
2377 * resolved along ECEF-frame axes and expressed in meters per
2378 * second (m/s).
2379 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
2380 * frame, resolved along ECEF-frame axes and expressed in meters
2381 * per second (m/s).
2382 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
2383 * frame, resolved along ECEF-frame axes and expressed in meters
2384 * per second (m/s).
2385 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
2386 * frame, resolved along ECEF-frame axes and expressed in meters
2387 * per second (m/s).
2388 * @param x cartesian x coordinate of body position expressed in meters (m)
2389 * with respect ECEF frame, resolved along ECEF-frame axes.
2390 * @param y cartesian y coordinate of body position expressed in meters (m)
2391 * with respect ECEF frame, resolved along ECEF-frame axes.
2392 * @param z cartesian z coordinate of body position expressed in meters (m)
2393 * with respect ECEF frame, resolved along ECEF-frame axes.
2394 * @return a new body kinematics instance.
2395 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
2396 * are not ECEF frame valid.
2397 */
2398 public static BodyKinematics estimateKinematicsAndReturnNew(
2399 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
2400 final double vx, final double vy, final double vz,
2401 final double oldVx, final double oldVy, final double oldVz,
2402 final double x, final double y, final double z) {
2403 final var result = new BodyKinematics();
2404 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z, result);
2405 return result;
2406 }
2407
2408 /**
2409 * Estimates body kinematics (specific force applied to a body and its angular rates).
2410 *
2411 * @param timeInterval time interval between.
2412 * @param c body-to-ECEF frame coordinate transformation matrix.
2413 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
2414 * @param vx velocity of body frame x coordinate with respect ECEF frame,
2415 * resolved along ECEF-frame axes and expressed in meters per
2416 * second (m/s).
2417 * @param vy velocity of body frame y coordinate with respect ECEF frame,
2418 * resolved along ECEF-frame axes and expressed in meters per
2419 * second (m/s).
2420 * @param vz velocity of body frame z coordinate with respect ECEF frame,
2421 * resolved along ECEF-frame axes and expressed in meters per
2422 * second (m/s).
2423 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
2424 * frame, resolved along ECEF-frame axes and expressed in meters
2425 * per second (m/s).
2426 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
2427 * frame, resolved along ECEF-frame axes and expressed in meters
2428 * per second (m/s).
2429 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
2430 * frame, resolved along ECEF-frame axes and expressed in meters
2431 * per second (m/s).
2432 * @param x cartesian x coordinate of body position expressed in meters (m)
2433 * with respect ECEF frame, resolved along ECEF-frame axes.
2434 * @param y cartesian y coordinate of body position expressed in meters (m)
2435 * with respect ECEF frame, resolved along ECEF-frame axes.
2436 * @param z cartesian z coordinate of body position expressed in meters (m)
2437 * with respect ECEF frame, resolved along ECEF-frame axes.
2438 * @return a new body kinematics instance.
2439 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
2440 * are not ECEF frame valid.
2441 */
2442 public static BodyKinematics estimateKinematicsAndReturnNew(
2443 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
2444 final double vx, final double vy, final double vz,
2445 final double oldVx, final double oldVy, final double oldVz,
2446 final double x, final double y, final double z) {
2447 final var result = new BodyKinematics();
2448 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z, result);
2449 return result;
2450 }
2451
2452 /**
2453 * Estimates body kinematics (specific force applied to a body and its angular rates).
2454 *
2455 * @param timeInterval time interval between epochs expressed in seconds (s).
2456 * @param c boy-to-ECEF frame coordinate transformation matrix.
2457 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
2458 * @param velocity velocity of body frame with respect ECEF frame.
2459 * @param oldVelocity previous velocity of body frame with respect ECEF frame.
2460 * @param x cartesian x coordinate of body position expressed in meters (m)
2461 * with respect ECEF frame, resolved along ECEF-frame axes.
2462 * @param y cartesian y coordinate of body position expressed in meters (m)
2463 * with respect ECEF frame, resolved along ECEF-frame axes.
2464 * @param z cartesian z coordinate of body position expressed in meters (m)
2465 * with respect ECEF frame, resolved along ECEF-frame axes.
2466 * @return a new body kinematics instance.
2467 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
2468 * are not ECEF frame valid.
2469 */
2470 public static BodyKinematics estimateKinematicsAndReturnNew(
2471 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
2472 final ECEFVelocity velocity, final ECEFVelocity oldVelocity,
2473 final double x, final double y, final double z) {
2474 final var result = new BodyKinematics();
2475 estimateKinematics(timeInterval, c, oldC, velocity, oldVelocity, x, y, z, result);
2476 return result;
2477 }
2478
2479 /**
2480 * Estimates body kinematics (specific force applied to a body and its angular rates).
2481 *
2482 * @param timeInterval time interval between epochs.
2483 * @param c boy-to-ECEF frame coordinate transformation matrix.
2484 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
2485 * @param velocity velocity of body frame with respect ECEF frame.
2486 * @param oldVelocity previous velocity of body frame with respect ECEF frame.
2487 * @param x cartesian x coordinate of body position expressed in meters (m)
2488 * with respect ECEF frame, resolved along ECEF-frame axes.
2489 * @param y cartesian y coordinate of body position expressed in meters (m)
2490 * with respect ECEF frame, resolved along ECEF-frame axes.
2491 * @param z cartesian z coordinate of body position expressed in meters (m)
2492 * with respect ECEF frame, resolved along ECEF-frame axes.
2493 * @return a new body kinematics instance.
2494 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
2495 * are not ECEF frame valid.
2496 */
2497 public static BodyKinematics estimateKinematicsAndReturnNew(
2498 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
2499 final ECEFVelocity velocity, final ECEFVelocity oldVelocity,
2500 final double x, final double y, final double z) {
2501 final var result = new BodyKinematics();
2502 estimateKinematics(timeInterval, c, oldC, velocity, oldVelocity, x, y, z, result);
2503 return result;
2504 }
2505
2506 /**
2507 * Estimates body kinematics (specific force applied to a body and its angular rates)..
2508 *
2509 * @param timeInterval time interval between epochs expressed in seconds (s).
2510 * @param c body-to-ECEF-frame coordinate transformation.
2511 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
2512 * @param vx x coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
2513 * along ECEF-frame axes.
2514 * @param vy y coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
2515 * along ECEF-frame axes.
2516 * @param vz z coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
2517 * along ECEF-frame axes.
2518 * @param oldVx x coordinate of previous velocity of body frame expressed in meters per second (m/s) and
2519 * resolved along ECEF-frame axes.
2520 * @param oldVy y coordinate of previous velocity of body frame expressed in meters per second (m/s) and
2521 * resolved along ECEF-frame axes.
2522 * @param oldVz z coordinate of previous velocity of body frame expressed in meters per second (m/s) and
2523 * resolved along ECEF-frame axes.
2524 * @param position cartesian body position resolved along ECEF-frame axes.
2525 * @return a new body kinematics instance.
2526 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
2527 * are not ECEF frame valid.
2528 */
2529 public static BodyKinematics estimateKinematicsAndReturnNew(
2530 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
2531 final double vx, final double vy, final double vz,
2532 final double oldVx, final double oldVy, final double oldVz, final ECEFPosition position) {
2533 final var result = new BodyKinematics();
2534 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position, result);
2535 return result;
2536 }
2537
2538 /**
2539 * Estimates body kinematics (specific force applied to a body and its angular rates)..
2540 *
2541 * @param timeInterval time interval between epochs.
2542 * @param c body-to-ECEF-frame coordinate transformation.
2543 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
2544 * @param vx x coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
2545 * along ECEF-frame axes.
2546 * @param vy y coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
2547 * along ECEF-frame axes.
2548 * @param vz z coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
2549 * along ECEF-frame axes.
2550 * @param oldVx x coordinate of previous velocity of body frame expressed in meters per second (m/s) and
2551 * resolved along ECEF-frame axes.
2552 * @param oldVy y coordinate of previous velocity of body frame expressed in meters per second (m/s) and
2553 * resolved along ECEF-frame axes.
2554 * @param oldVz z coordinate of previous velocity of body frame expressed in meters per second (m/s) and
2555 * resolved along ECEF-frame axes.
2556 * @param position cartesian body position resolved along ECEF-frame axes.
2557 * @return a new body kinematics instance.
2558 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
2559 * are not ECEF frame valid.
2560 */
2561 public static BodyKinematics estimateKinematicsAndReturnNew(
2562 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
2563 final double vx, final double vy, final double vz,
2564 final double oldVx, final double oldVy, final double oldVz, final ECEFPosition position) {
2565 final var result = new BodyKinematics();
2566 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position, result);
2567 return result;
2568 }
2569
2570 /**
2571 * Estimates body kinematics (specific force applied to a body and its angular rates).
2572 *
2573 * @param timeInterval time interval between epochs expressed in seconds (s).
2574 * @param c boy-to-ECEF frame coordinate transformation matrix.
2575 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
2576 * @param velocity velocity of body frame with respect ECEF frame.
2577 * @param oldVelocity previous velocity of body frame with respect ECEF frame.
2578 * @param position cartesian body position resolved along ECEF-frame axes.
2579 * @return a new body kinematics instance.
2580 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
2581 * are not ECEF frame valid.
2582 */
2583 public static BodyKinematics estimateKinematicsAndReturnNew(
2584 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
2585 final ECEFVelocity velocity, final ECEFVelocity oldVelocity, final ECEFPosition position) {
2586 final var result = new BodyKinematics();
2587 estimateKinematics(timeInterval, c, oldC, velocity, oldVelocity, position, result);
2588 return result;
2589 }
2590
2591 /**
2592 * Estimates body kinematics (specific force applied to a body and its angular rates).
2593 *
2594 * @param timeInterval time interval between epochs.
2595 * @param c boy-to-ECEF frame coordinate transformation matrix.
2596 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
2597 * @param velocity velocity of body frame with respect ECEF frame.
2598 * @param oldVelocity previous velocity of body frame with respect ECEF frame.
2599 * @param position cartesian body position resolved along ECEF-frame axes.
2600 * @return a new body kinematics instance.
2601 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
2602 * are not ECEF frame valid.
2603 */
2604 public static BodyKinematics estimateKinematicsAndReturnNew(
2605 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
2606 final ECEFVelocity velocity, final ECEFVelocity oldVelocity, final ECEFPosition position) {
2607 final var result = new BodyKinematics();
2608 estimateKinematics(timeInterval, c, oldC, velocity, oldVelocity, position, result);
2609 return result;
2610 }
2611
2612 /**
2613 * Estimates body kinematics (specific force applied to a body and its angular rates).
2614 *
2615 * @param timeInterval time interval between epochs expressed in seconds (s).
2616 * @param frame body ECEF frame containing current position, velocity and
2617 * body-to-ECEF frame coordinate transformation.
2618 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
2619 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
2620 * frame, resolved along ECEF-frame axes and expressed in meters
2621 * per second (m/s).
2622 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
2623 * frame, resolved along ECEF-frame axes and expressed in meters
2624 * per second (m/s).
2625 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
2626 * frame, resolved along ECEF-frame axes and expressed in meters
2627 * per second (m/s).
2628 * @return a new body kinematics instance.
2629 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
2630 * are not ECEF frame valid.
2631 */
2632 public static BodyKinematics estimateKinematicsAndReturnNew(
2633 final double timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
2634 final double oldVx, final double oldVy, final double oldVz) {
2635 final var result = new BodyKinematics();
2636 estimateKinematics(timeInterval, frame, oldC, oldVx, oldVy, oldVz, result);
2637 return result;
2638 }
2639
2640 /**
2641 * Estimates body kinematics (specific force applied to a body and its angular rates).
2642 *
2643 * @param timeInterval time interval between epochs.
2644 * @param frame body ECEF frame containing current position, velocity and
2645 * body-to-ECEF frame coordinate transformation.
2646 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
2647 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
2648 * frame, resolved along ECEF-frame axes and expressed in meters
2649 * per second (m/s).
2650 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
2651 * frame, resolved along ECEF-frame axes and expressed in meters
2652 * per second (m/s).
2653 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
2654 * frame, resolved along ECEF-frame axes and expressed in meters
2655 * per second (m/s).
2656 * @return a new body kinematics instance.
2657 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
2658 * are not ECEF frame valid.
2659 */
2660 public static BodyKinematics estimateKinematicsAndReturnNew(
2661 final Time timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
2662 final double oldVx, final double oldVy, final double oldVz) {
2663 final var result = new BodyKinematics();
2664 estimateKinematics(timeInterval, frame, oldC, oldVx, oldVy, oldVz, result);
2665 return result;
2666 }
2667
2668 /**
2669 * Estimates body kinematics (specific force applied to a body and its angular rates).
2670 *
2671 * @param timeInterval time interval between epochs expressed in seconds (s).
2672 * @param frame body ECEF frame containing current position, velocity and
2673 * body-to-ECEF frame coordinate transformation.
2674 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
2675 * @param oldVelocity previous velocity of body frame with respect ECEF frame.
2676 * @return a new body kinematics instance.
2677 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
2678 * are not ECEF frame valid.
2679 */
2680 public static BodyKinematics estimateKinematicsAndReturnNew(
2681 final double timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
2682 final ECEFVelocity oldVelocity) {
2683 final var result = new BodyKinematics();
2684 estimateKinematics(timeInterval, frame, oldC, oldVelocity, result);
2685 return result;
2686 }
2687
2688 /**
2689 * Estimates body kinematics (specific force applied to a body and its angular rates).
2690 *
2691 * @param timeInterval time interval between epochs.
2692 * @param frame body ECEF frame containing current position, velocity and
2693 * body-to-ECEF frame coordinate transformation.
2694 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
2695 * @param oldVelocity previous velocity of body frame with respect ECEF frame.
2696 * @return a new body kinematics instance.
2697 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
2698 * are not ECEF frame valid.
2699 */
2700 public static BodyKinematics estimateKinematicsAndReturnNew(
2701 final Time timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
2702 final ECEFVelocity oldVelocity) {
2703 final var result = new BodyKinematics();
2704 estimateKinematics(timeInterval, frame, oldC, oldVelocity, result);
2705 return result;
2706 }
2707
2708 /**
2709 * Estimates body kinematics (specific force applied to a body and its angular rates).
2710 *
2711 * @param timeInterval time interval between epochs expressed in seconds (s).
2712 * @param frame body ECEF frame containing current position, velocity and
2713 * body-to-ECEF frame coordinate transformation.
2714 * @param oldFrame body ECEF frame containing previous position, velocity and
2715 * body-to-ECEF frame coordinate transformation. Notice that
2716 * previous position contained in this frame is ignored.
2717 * @return a new body kinematics instance.
2718 * @throws IllegalArgumentException if provided time interval is negative.
2719 */
2720 public static BodyKinematics estimateKinematicsAndReturnNew(
2721 final double timeInterval, final ECEFFrame frame, final ECEFFrame oldFrame) {
2722 final var result = new BodyKinematics();
2723 estimateKinematics(timeInterval, frame, oldFrame, result);
2724 return result;
2725 }
2726
2727 /**
2728 * Estimates body kinematics (specific force applied to a body and its angular rates).
2729 *
2730 * @param timeInterval time interval between epochs.
2731 * @param frame body ECEF frame containing current position, velocity and
2732 * body-to-ECEF frame coordinate transformation.
2733 * @param oldFrame body ECEF frame containing previous position, velocity and
2734 * body-to-ECEF frame coordinate transformation. Notice that
2735 * previous position contained in this frame is ignored.
2736 * @return a new body kinematics instance.
2737 * @throws IllegalArgumentException if provided time interval is negative.
2738 */
2739 public static BodyKinematics estimateKinematicsAndReturnNew(
2740 final Time timeInterval, final ECEFFrame frame, final ECEFFrame oldFrame) {
2741 final var result = new BodyKinematics();
2742 estimateKinematics(timeInterval, frame, oldFrame, result);
2743 return result;
2744 }
2745
2746 /**
2747 * Estimates body kinematics (specific force applied to a body and its angular rates).
2748 *
2749 * @param timeInterval time interval between epochs expressed in seconds (s).
2750 * @param c body-to-ECEF frame coordinate transformation matrix.
2751 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
2752 * @param vx velocity of body frame x coordinate with respect ECEF frame,
2753 * resolved along ECEF-frame axes.
2754 * @param vy velocity of body frame y coordinate with respect ECEF frame,
2755 * resolved along ECEF-frame axes.
2756 * @param vz velocity of body frame z coordinate with respect ECEF frame,
2757 * resolved along ECEF-frame axes.
2758 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
2759 * frame, resolved along ECEF-frame axes.
2760 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
2761 * frame, resolved along ECEF-frame axes.
2762 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
2763 * frame, resolved along ECEF-frame axes.
2764 * @param x cartesian x coordinate of body position expressed in meters (m)
2765 * with respect ECEF frame, resolved along ECEF-frame axes.
2766 * @param y cartesian y coordinate of body position expressed in meters (m)
2767 * with respect ECEF frame, resolved along ECEF-frame axes.
2768 * @param z cartesian z coordinate of body position expressed in meters (m)
2769 * with respect ECEF frame, resolved along ECEF-frame axes.
2770 * @return a new body kinematics instance.
2771 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
2772 * are not ECEF frame valid.
2773 */
2774 public static BodyKinematics estimateKinematicsAndReturnNew(
2775 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
2776 final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
2777 final double x, final double y, final double z) {
2778 final var result = new BodyKinematics();
2779 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z, result);
2780 return result;
2781 }
2782
2783 /**
2784 * Estimates body kinematics (specific force applied to a body and its angular rates).
2785 *
2786 * @param timeInterval time interval between epochs.
2787 * @param c body-to-ECEF frame coordinate transformation matrix.
2788 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
2789 * @param vx velocity of body frame x coordinate with respect ECEF frame,
2790 * resolved along ECEF-frame axes.
2791 * @param vy velocity of body frame y coordinate with respect ECEF frame,
2792 * resolved along ECEF-frame axes.
2793 * @param vz velocity of body frame z coordinate with respect ECEF frame,
2794 * resolved along ECEF-frame axes.
2795 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
2796 * frame, resolved along ECEF-frame axes.
2797 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
2798 * frame, resolved along ECEF-frame axes.
2799 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
2800 * frame, resolved along ECEF-frame axes.
2801 * @param x cartesian x coordinate of body position expressed in meters (m)
2802 * with respect ECEF frame, resolved along ECEF-frame axes.
2803 * @param y cartesian y coordinate of body position expressed in meters (m)
2804 * with respect ECEF frame, resolved along ECEF-frame axes.
2805 * @param z cartesian z coordinate of body position expressed in meters (m)
2806 * with respect ECEF frame, resolved along ECEF-frame axes.
2807 * @return a new body kinematics instance.
2808 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
2809 * are not ECEF frame valid.
2810 */
2811 public static BodyKinematics estimateKinematicsAndReturnNew(
2812 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
2813 final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
2814 final double x, final double y, final double z) {
2815 final var result = new BodyKinematics();
2816 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z, result);
2817 return result;
2818 }
2819
2820 /**
2821 * Estimates body kinematics (specific force applied to a body and its angular rates).
2822 *
2823 * @param timeInterval time interval between epochs expressed in seconds (s).
2824 * @param c body-to-ECEF frame coordinate transformation matrix.
2825 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
2826 * @param vx velocity of body frame x coordinate with respect ECEF frame,
2827 * resolved along ECEF-frame axes.
2828 * @param vy velocity of body frame y coordinate with respect ECEF frame,
2829 * resolved along ECEF-frame axes.
2830 * @param vz velocity of body frame z coordinate with respect ECEF frame,
2831 * resolved along ECEF-frame axes.
2832 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
2833 * frame, resolved along ECEF-frame axes.
2834 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
2835 * frame, resolved along ECEF-frame axes.
2836 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
2837 * frame, resolved along ECEF-frame axes.
2838 * @param position cartesian body position resolved along ECEF-frame axes.
2839 * @return a new body kinematics instance.
2840 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
2841 * are not ECEF frame valid.
2842 */
2843 public static BodyKinematics estimateKinematicsAndReturnNew(
2844 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
2845 final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
2846 final ECEFPosition position) {
2847 final var result = new BodyKinematics();
2848 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position, result);
2849 return result;
2850 }
2851
2852 /**
2853 * Estimates body kinematics (specific force applied to a body and its angular rates).
2854 *
2855 * @param timeInterval time interval between epochs.
2856 * @param c body-to-ECEF frame coordinate transformation matrix.
2857 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
2858 * @param vx velocity of body frame x coordinate with respect ECEF frame,
2859 * resolved along ECEF-frame axes.
2860 * @param vy velocity of body frame y coordinate with respect ECEF frame,
2861 * resolved along ECEF-frame axes.
2862 * @param vz velocity of body frame z coordinate with respect ECEF frame,
2863 * resolved along ECEF-frame axes.
2864 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
2865 * frame, resolved along ECEF-frame axes.
2866 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
2867 * frame, resolved along ECEF-frame axes.
2868 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
2869 * frame, resolved along ECEF-frame axes.
2870 * @param position cartesian body position resolved along ECEF-frame axes.
2871 * @return a new body kinematics instance.
2872 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
2873 * are not ECEF frame valid.
2874 */
2875 public static BodyKinematics estimateKinematicsAndReturnNew(
2876 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
2877 final Speed vx, final Speed vy, final Speed vz,
2878 final Speed oldVx, final Speed oldVy, final Speed oldVz, final ECEFPosition position) {
2879 final var result = new BodyKinematics();
2880 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position, result);
2881 return result;
2882 }
2883
2884 /**
2885 * Estimates body kinematics (specific force applied to a body and its angular rates).
2886 *
2887 * @param timeInterval time interval between epochs expressed in seconds (s).
2888 * @param frame body ECEF frame containing current position, velocity and
2889 * body-to-ECEF frame coordinate transformation.
2890 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
2891 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
2892 * frame, resolved along ECEF-frame axes.
2893 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
2894 * frame, resolved along ECEF-frame axes.
2895 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
2896 * frame, resolved along ECEF-frame axes.
2897 * @return a new body kinematics instance.
2898 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
2899 * are not ECEF frame valid.
2900 */
2901 public static BodyKinematics estimateKinematicsAndReturnNew(
2902 final double timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
2903 final Speed oldVx, final Speed oldVy, final Speed oldVz) {
2904 final var result = new BodyKinematics();
2905 estimateKinematics(timeInterval, frame, oldC, oldVx, oldVy, oldVz, result);
2906 return result;
2907 }
2908
2909 /**
2910 * Estimates body kinematics (specific force applied to a body and its angular rates).
2911 *
2912 * @param timeInterval time interval between epochs.
2913 * @param frame body ECEF frame containing current position, velocity and
2914 * body-to-ECEF frame coordinate transformation.
2915 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
2916 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
2917 * frame, resolved along ECEF-frame axes.
2918 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
2919 * frame, resolved along ECEF-frame axes.
2920 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
2921 * frame, resolved along ECEF-frame axes.
2922 * @return a new body kinematics instance.
2923 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
2924 * are not ECEF frame valid.
2925 */
2926 public static BodyKinematics estimateKinematicsAndReturnNew(
2927 final Time timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
2928 final Speed oldVx, final Speed oldVy, final Speed oldVz) {
2929 final var result = new BodyKinematics();
2930 estimateKinematics(timeInterval, frame, oldC, oldVx, oldVy, oldVz, result);
2931 return result;
2932 }
2933
2934 /**
2935 * Estimates body kinematics (specific force applied to a body and its angular rates).
2936 *
2937 * @param timeInterval time interval between epochs expressed in seconds (s).
2938 * @param c body-to-ECEF frame coordinate transformation matrix.
2939 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
2940 * @param vx velocity of body frame x coordinate with respect ECEF frame,
2941 * resolved along ECEF-frame axes and expressed in meters per
2942 * second (m/s).
2943 * @param vy velocity of body frame y coordinate with respect ECEF frame,
2944 * resolved along ECEF-frame axes and expressed in meters per
2945 * second (m/s).
2946 * @param vz velocity of body frame z coordinate with respect ECEF frame,
2947 * resolved along ECEF-frame axes and expressed in meters per
2948 * second (m/s).
2949 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
2950 * frame, resolved along ECEF-frame axes and expressed in meters
2951 * per second (m/s).
2952 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
2953 * frame, resolved along ECEF-frame axes and expressed in meters
2954 * per second (m/s).
2955 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
2956 * frame, resolved along ECEF-frame axes and expressed in meters
2957 * per second (m/s).
2958 * @param position body position expressed in meters (m) with respect ECEF frame,
2959 * resolved along ECEF-frame axes.
2960 * @return a new body kinematics instance.
2961 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
2962 * are not ECEF frame valid.
2963 */
2964 public static BodyKinematics estimateKinematicsAndReturnNew(
2965 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
2966 final double vx, final double vy, final double vz,
2967 final double oldVx, final double oldVy, final double oldVz, final Point3D position) {
2968 return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz,
2969 position.getInhomX(), position.getInhomY(), position.getInhomZ());
2970 }
2971
2972 /**
2973 * Estimates body kinematics (specific force applied to a body and its angular rates).
2974 *
2975 * @param timeInterval time interval between epochs.
2976 * @param c body-to-ECEF frame coordinate transformation matrix.
2977 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
2978 * @param vx velocity of body frame x coordinate with respect ECEF frame,
2979 * resolved along ECEF-frame axes and expressed in meters per
2980 * second (m/s).
2981 * @param vy velocity of body frame y coordinate with respect ECEF frame,
2982 * resolved along ECEF-frame axes and expressed in meters per
2983 * second (m/s).
2984 * @param vz velocity of body frame z coordinate with respect ECEF frame,
2985 * resolved along ECEF-frame axes and expressed in meters per
2986 * second (m/s).
2987 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
2988 * frame, resolved along ECEF-frame axes and expressed in meters
2989 * per second (m/s).
2990 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
2991 * frame, resolved along ECEF-frame axes and expressed in meters
2992 * per second (m/s).
2993 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
2994 * frame, resolved along ECEF-frame axes and expressed in meters
2995 * per second (m/s).
2996 * @param position body position expressed in meters (m) with respect ECEF frame,
2997 * resolved along ECEF-frame axes.
2998 * @return a new body kinematics instance.
2999 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
3000 * are not ECEF frame valid.
3001 */
3002 public static BodyKinematics estimateKinematicsAndReturnNew(
3003 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
3004 final double vx, final double vy, final double vz,
3005 final double oldVx, final double oldVy, final double oldVz, final Point3D position) {
3006 return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz,
3007 position.getInhomX(), position.getInhomY(), position.getInhomZ());
3008 }
3009
3010 /**
3011 * Estimates body kinematics (specific force applied to a body and its angular rates).
3012 *
3013 * @param timeInterval time interval between epochs expressed in seconds (s).
3014 * @param c body-to-ECEF frame coordinate transformation matrix.
3015 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
3016 * @param velocity velocity of body frame with respect ECEF frame.
3017 * @param oldVelocity previous velocity of body frame with respect ECEF frame.
3018 * @param position body position expressed in meters (m) with respect ECEF frame,
3019 * resolved along ECEF-frame axes.
3020 * @return a new body kinematics instance.
3021 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
3022 * are not ECEF frame valid.
3023 */
3024 public static BodyKinematics estimateKinematicsAndReturnNew(
3025 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
3026 final ECEFVelocity velocity, final ECEFVelocity oldVelocity, final Point3D position) {
3027 final var result = new BodyKinematics();
3028 estimateKinematics(timeInterval, c, oldC, velocity, oldVelocity, position, result);
3029 return result;
3030 }
3031
3032 /**
3033 * Estimates body kinematics (specific force applied to a body and its angular rates).
3034 *
3035 * @param timeInterval time interval between epochs.
3036 * @param c body-to-ECEF frame coordinate transformation matrix.
3037 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
3038 * @param velocity velocity of body frame with respect ECEF frame.
3039 * @param oldVelocity previous velocity of body frame with respect ECEF frame.
3040 * @param position body position expressed in meters (m) with respect ECEF frame,
3041 * resolved along ECEF-frame axes.
3042 * @return a new body kinematics instance.
3043 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
3044 * are not ECEF frame valid.
3045 */
3046 public static BodyKinematics estimateKinematicsAndReturnNew(
3047 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
3048 final ECEFVelocity velocity, final ECEFVelocity oldVelocity, final Point3D position) {
3049 final var result = new BodyKinematics();
3050 estimateKinematics(timeInterval, c, oldC, velocity, oldVelocity, position, result);
3051 return result;
3052 }
3053
3054 /**
3055 * Estimates body kinematics (specific force applied to a body and its angular rates).
3056 *
3057 * @param timeInterval time interval between epochs expressed in seconds (s).
3058 * @param c boy-to-ECEF frame coordinate transformation matrix.
3059 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
3060 * @param vx velocity of body frame x coordinate with respect ECEF frame,
3061 * resolved along ECEF-frame axes.
3062 * @param vy velocity of body frame y coordinate with respect ECEF frame,
3063 * resolved along ECEF-frame axes.
3064 * @param vz velocity of body frame z coordinate with respect ECEF frame,
3065 * resolved along ECEF-frame axes.
3066 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
3067 * frame, resolved along ECEF-frame axes.
3068 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
3069 * frame, resolved along ECEF-frame axes.
3070 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
3071 * frame, resolved along ECEF-frame axes.
3072 * @param position body position expressed in meters (m) with respect ECEF frame,
3073 * resolved along ECEF-frame axes.
3074 * @return a new body kinematics instance.
3075 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
3076 * are not ECEF frame valid.
3077 */
3078 public static BodyKinematics estimateKinematicsAndReturnNew(
3079 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
3080 final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
3081 final Point3D position) {
3082 return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz,
3083 position.getInhomX(), position.getInhomY(), position.getInhomZ());
3084 }
3085
3086 /**
3087 * Estimates body kinematics (specific force applied to a body and its angular rates).
3088 *
3089 * @param timeInterval time interval between epochs.
3090 * @param c boy-to-ECEF frame coordinate transformation matrix.
3091 * @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
3092 * @param vx velocity of body frame x coordinate with respect ECEF frame,
3093 * resolved along ECEF-frame axes.
3094 * @param vy velocity of body frame y coordinate with respect ECEF frame,
3095 * resolved along ECEF-frame axes.
3096 * @param vz velocity of body frame z coordinate with respect ECEF frame,
3097 * resolved along ECEF-frame axes.
3098 * @param oldVx previous velocity of body frame x coordinate with respect ECEF
3099 * frame, resolved along ECEF-frame axes.
3100 * @param oldVy previous velocity of body frame y coordinate with respect ECEF
3101 * frame, resolved along ECEF-frame axes.
3102 * @param oldVz previous velocity of body frame z coordinate with respect ECEF
3103 * frame, resolved along ECEF-frame axes.
3104 * @param position body position expressed in meters (m) with respect ECEF frame,
3105 * resolved along ECEF-frame axes.
3106 * @return a new body kinematics instance.
3107 * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
3108 * are not ECEF frame valid.
3109 */
3110 public static BodyKinematics estimateKinematicsAndReturnNew(
3111 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
3112 final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
3113 final Point3D position) {
3114 return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz,
3115 position.getInhomX(), position.getInhomY(), position.getInhomZ());
3116 }
3117 }