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.ECIFrame;
24 import com.irurueta.navigation.inertial.BodyKinematics;
25 import com.irurueta.units.Speed;
26 import com.irurueta.units.SpeedConverter;
27 import com.irurueta.units.SpeedUnit;
28 import com.irurueta.units.Time;
29 import com.irurueta.units.TimeConverter;
30 import com.irurueta.units.TimeUnit;
31
32 /**
33 * Estimates body kinematics (specific force applied to a body and its angular rates) with respect and resolved
34 * along ECI-frame axes.
35 * This implementation is based on the equations defined in "Principles of GNSS, Inertial, and Multisensor
36 * Integrated Navigation Systems, Second Edition" and on the companion software available at:
37 * <a href="https://github.com/ymjdz/MATLAB-Codes/blob/master/Kinematics_ECI.m">
38 * https://github.com/ymjdz/MATLAB-Codes/blob/master/Kinematics_ECI.m
39 * </a>
40 */
41 public class ECIKinematicsEstimator {
42
43 /**
44 * Scaling threshold.
45 */
46 private static final double SCALING_THRESHOLD = 2e-5;
47
48 /**
49 * Alpha threshold.
50 */
51 private static final double ALPHA_THRESHOLD = 1e-8;
52
53 /**
54 * Number of rows.
55 */
56 private static final int ROWS = 3;
57
58 /**
59 * Estimates body kinematics (specific force applied to a body and its angular rates)..
60 *
61 * @param timeInterval time interval between epochs expressed in seconds (s).
62 * @param c body-to-ECI-frame coordinate transformation.
63 * @param oldC previous body-to-ECI-frame coordinate transformation.
64 * @param vx x coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
65 * along ECI-frame axes.
66 * @param vy y coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
67 * along ECI-frame axes.
68 * @param vz z coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
69 * along ECI-frame axes.
70 * @param oldVx x coordinate of previous velocity of body frame expressed in meters per second (m/s) and
71 * resolved along ECI-frame axes.
72 * @param oldVy y coordinate of previous velocity of body frame expressed in meters per second (m/s) and
73 * resolved along ECI-frame axes.
74 * @param oldVz z coordinate of previous velocity of body frame expressed in meters per second (m/s) and
75 * resolved along ECI-frame axes.
76 * @param x cartesian x coordinate of body position expressed in meters (m) and resolved along
77 * ECI-frame axes.
78 * @param y cartesian y coordinate of body position expressed in meters (m) and resolved along
79 * ECI-frame axes.
80 * @param z cartesian z coordinate of body position expressed in meters (m) and resolved along
81 * ECI-frame axes.
82 * @param result instance where estimated body kinematics will be stored.
83 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
84 * are not ECI frame valid.
85 */
86 public void estimate(
87 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
88 final double vx, final double vy, final double vz,
89 final double oldVx, final double oldVy, final double oldVz,
90 final double x, final double y, final double z, final BodyKinematics result) {
91 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z, result);
92 }
93
94 /**
95 * Estimates body kinematics (specific force applied to a body and its angular rates)..
96 *
97 * @param timeInterval time interval between epochs.
98 * @param c body-to-ECI-frame coordinate transformation.
99 * @param oldC previous body-to-ECI-frame coordinate transformation.
100 * @param vx x coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
101 * along ECI-frame axes.
102 * @param vy y coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
103 * along ECI-frame axes.
104 * @param vz z coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
105 * along ECI-frame axes.
106 * @param oldVx x coordinate of previous velocity of body frame expressed in meters per second (m/s) and
107 * resolved along ECI-frame axes.
108 * @param oldVy y coordinate of previous velocity of body frame expressed in meters per second (m/s) and
109 * resolved along ECI-frame axes.
110 * @param oldVz z coordinate of previous velocity of body frame expressed in meters per second (m/s) and
111 * resolved along ECI-frame axes.
112 * @param x cartesian x coordinate of body position expressed in meters (m) and resolved along
113 * ECI-frame axes.
114 * @param y cartesian y coordinate of body position expressed in meters (m) and resolved along
115 * ECI-frame axes.
116 * @param z cartesian z coordinate of body position expressed in meters (m) and resolved along
117 * ECI-frame axes.
118 * @param result instance where estimated body kinematics will be stored.
119 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
120 * are not ECI frame valid.
121 */
122 public void estimate(
123 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
124 final double vx, final double vy, final double vz,
125 final double oldVx, final double oldVy, final double oldVz,
126 final double x, final double y, final double z, final BodyKinematics result) {
127 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z, result);
128 }
129
130 /**
131 * Estimates body kinematics (specific force applied to a body and its angular rates).
132 *
133 * @param timeInterval time interval between epochs expressed in seconds (s).
134 * @param frame body ECI frame containing current position, velocity and
135 * body-to-ECI frame coordinate transformation.
136 * @param oldC previous body-to-ECI-frame coordinate transformation.
137 * @param oldVx x coordinate of previous velocity of body frame expressed in meters per second (m/s) and
138 * resolved along ECI-frame axes.
139 * @param oldVy y coordinate of previous velocity of body frame expressed in meters per second (m/s) and
140 * resolved along ECI-frame axes.
141 * @param oldVz z coordinate of previous velocity of body frame expressed in meters per second (m/s) and
142 * resolved along ECI-frame axes.
143 * @param result instance where estimated body kinematics will be stored.
144 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
145 * are not ECI frame valid.
146 */
147 public void estimate(
148 final double timeInterval, final ECIFrame frame, final CoordinateTransformation oldC,
149 final double oldVx, final double oldVy, final double oldVz, final BodyKinematics result) {
150 estimateKinematics(timeInterval, frame, oldC, oldVx, oldVy, oldVz, result);
151 }
152
153 /**
154 * Estimates body kinematics (specific force applied to a body and its angular rates).
155 *
156 * @param timeInterval time interval between epochs.
157 * @param frame body ECI frame containing current position, velocity and
158 * body-to-ECI frame coordinate transformation.
159 * @param oldC previous body-to-ECI-frame coordinate transformation.
160 * @param oldVx x coordinate of previous velocity of body frame expressed in meters per second (m/s) and
161 * resolved along ECI-frame axes.
162 * @param oldVy y coordinate of previous velocity of body frame expressed in meters per second (m/s) and
163 * resolved along ECI-frame axes.
164 * @param oldVz z coordinate of previous velocity of body frame expressed in meters per second (m/s) and
165 * resolved along ECI-frame axes.
166 * @param result instance where estimated body kinematics will be stored.
167 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
168 * are not ECI frame valid.
169 */
170 public void estimate(
171 final Time timeInterval, final ECIFrame frame, final CoordinateTransformation oldC,
172 final double oldVx, final double oldVy, final double oldVz, final BodyKinematics result) {
173 estimateKinematics(timeInterval, frame, oldC, oldVx, oldVy, oldVz, result);
174 }
175
176 /**
177 * Estimates body kinematics (specific force applied to a body and its angular rates).
178 *
179 * @param timeInterval time interval between epochs expressed in seconds (s).
180 * @param frame body ECI frame containing current position, velocity and
181 * body-to-ECI frame coordinate transformation.
182 * @param oldFrame body ECI frame containing previous position, velocity and
183 * body-to-ECI frame coordinate transformation. Notice that
184 * previous position contained in this frame is ignored.
185 * @param result instance where body kinematics estimation will be stored.
186 * @throws IllegalArgumentException if provided time interval is negative.
187 */
188 public void estimate(
189 final double timeInterval, final ECIFrame frame, final ECIFrame oldFrame, final BodyKinematics result) {
190 estimateKinematics(timeInterval, frame, oldFrame, result);
191 }
192
193 /**
194 * Estimates body kinematics (specific force applied to a body and its angular rates).
195 *
196 * @param timeInterval time interval between epochs.
197 * @param frame body ECI frame containing current position, velocity and
198 * body-to-ECI frame coordinate transformation.
199 * @param oldFrame body ECI frame containing previous position, velocity and
200 * body-to-ECI frame coordinate transformation. Notice that
201 * previous position contained in this frame is ignored.
202 * @param result instance where body kinematics estimation will be stored.
203 * @throws IllegalArgumentException if provided time interval is negative.
204 */
205 public void estimate(
206 final Time timeInterval, final ECIFrame frame, final ECIFrame oldFrame, final BodyKinematics result) {
207 estimateKinematics(timeInterval, frame, oldFrame, result);
208 }
209
210 /**
211 * Estimates body kinematics (specific force applied to a body and its angular rates)..
212 *
213 * @param timeInterval time interval between epochs expressed in seconds (s).
214 * @param c body-to-ECI-frame coordinate transformation.
215 * @param oldC previous body-to-ECI-frame coordinate transformation.
216 * @param vx x coordinate of velocity of body frame with respect ECI frame, resolved
217 * along ECI-frame axes.
218 * @param vy y coordinate of velocity of body frame with respect ECI frame, resolved
219 * along ECI-frame axes.
220 * @param vz z coordinate of velocity of body frame with respect ECI frame, resolved
221 * along ECI-frame axes.
222 * @param oldVx x coordinate of previous velocity of body frame with respect ECI frame,
223 * resolved along ECI-frame axes.
224 * @param oldVy y coordinate of previous velocity of body frame with respect ECI frame,
225 * resolved along ECI-frame axes.
226 * @param oldVz z coordinate of previous velocity of body frame with respect ECI frame,
227 * resolved along ECI-frame axes.
228 * @param x cartesian x coordinate of body position expressed in meters (m) and resolved along
229 * ECI-frame axes.
230 * @param y cartesian y coordinate of body position expressed in meters (m) and resolved along
231 * ECI-frame axes.
232 * @param z cartesian z coordinate of body position expressed in meters (m) and resolved along
233 * ECI-frame axes.
234 * @param result instance where estimated body kinematics will be stored.
235 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
236 * are not ECI frame valid.
237 */
238 public void estimate(
239 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
240 final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
241 final double x, final double y, final double z, final BodyKinematics result) {
242 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z, result);
243 }
244
245 /**
246 * Estimates body kinematics (specific force applied to a body and its angular rates)..
247 *
248 * @param timeInterval time interval between epochs.
249 * @param c body-to-ECI-frame coordinate transformation.
250 * @param oldC previous body-to-ECI-frame coordinate transformation.
251 * @param vx x coordinate of velocity of body frame with respect ECI frame, resolved
252 * along ECI-frame axes.
253 * @param vy y coordinate of velocity of body frame with respect ECI frame, resolved
254 * along ECI-frame axes.
255 * @param vz z coordinate of velocity of body frame with respect ECI frame, resolved
256 * along ECI-frame axes.
257 * @param oldVx x coordinate of previous velocity of body frame with respect ECI frame,
258 * resolved along ECI-frame axes.
259 * @param oldVy y coordinate of previous velocity of body frame with respect ECI frame,
260 * resolved along ECI-frame axes.
261 * @param oldVz z coordinate of previous velocity of body frame with respect ECI frame,
262 * resolved along ECI-frame axes.
263 * @param x cartesian x coordinate of body position expressed in meters (m) and resolved along
264 * ECI-frame axes.
265 * @param y cartesian y coordinate of body position expressed in meters (m) and resolved along
266 * ECI-frame axes.
267 * @param z cartesian z coordinate of body position expressed in meters (m) and resolved along
268 * ECI-frame axes.
269 * @param result instance where estimated body kinematics will be stored.
270 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
271 * are not ECI frame valid.
272 */
273 public void estimate(
274 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
275 final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
276 final double x, final double y, final double z, final BodyKinematics result) {
277 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z, result);
278 }
279
280 /**
281 * Estimates body kinematics (specific force applied to a body and its angular rates).
282 *
283 * @param timeInterval time interval between epochs expressed in seconds (s).
284 * @param frame body ECI frame containing current position, velocity and
285 * body-to-ECI frame coordinate transformation.
286 * @param oldC previous body-to-ECI-frame coordinate transformation.
287 * @param oldVx x coordinate of previous velocity of body frame with respect ECI frame,
288 * resolved along ECI-frame axes.
289 * @param oldVy y coordinate of previous velocity of body frame with respect ECI frame,
290 * resolved along ECI-frame axes.
291 * @param oldVz z coordinate of previous velocity of body frame with respect ECI frame,
292 * resolved along ECI-frame axes.
293 * @param result instance where estimated body kinematics will be stored.
294 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
295 * are not ECI frame valid.
296 */
297 public void estimate(
298 final double timeInterval, final ECIFrame frame, final CoordinateTransformation oldC,
299 final Speed oldVx, final Speed oldVy, final Speed oldVz, final BodyKinematics result) {
300 estimateKinematics(timeInterval, frame, oldC, oldVx, oldVy, oldVz, result);
301 }
302
303 /**
304 * Estimates body kinematics (specific force applied to a body and its angular rates).
305 *
306 * @param timeInterval time interval between epochs.
307 * @param frame body ECI frame containing current position, velocity and
308 * body-to-ECI frame coordinate transformation.
309 * @param oldC previous body-to-ECI-frame coordinate transformation.
310 * @param oldVx x coordinate of previous velocity of body frame with respect ECI frame,
311 * resolved along ECI-frame axes.
312 * @param oldVy y coordinate of previous velocity of body frame with respect ECI frame,
313 * resolved along ECI-frame axes.
314 * @param oldVz z coordinate of previous velocity of body frame with respect ECI frame,
315 * resolved along ECI-frame axes.
316 * @param result instance where estimated body kinematics will be stored.
317 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
318 * are not ECI frame valid.
319 */
320 public void estimate(
321 final Time timeInterval, final ECIFrame frame, final CoordinateTransformation oldC,
322 final Speed oldVx, final Speed oldVy, final Speed oldVz, final BodyKinematics result) {
323 estimateKinematics(timeInterval, frame, oldC, oldVx, oldVy, oldVz, result);
324 }
325
326 /**
327 * Estimates body kinematics (specific force applied to a body and its angular rates)..
328 *
329 * @param timeInterval time interval between epochs expressed in seconds (s).
330 * @param c body-to-ECI-frame coordinate transformation.
331 * @param oldC previous body-to-ECI-frame coordinate transformation.
332 * @param vx x coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
333 * along ECI-frame axes.
334 * @param vy y coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
335 * along ECI-frame axes.
336 * @param vz z coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
337 * along ECI-frame axes.
338 * @param oldVx x coordinate of previous velocity of body frame expressed in meters per second (m/s) and
339 * resolved along ECI-frame axes.
340 * @param oldVy y coordinate of previous velocity of body frame expressed in meters per second (m/s) and
341 * resolved along ECI-frame axes.
342 * @param oldVz z coordinate of previous velocity of body frame expressed in meters per second (m/s) and
343 * resolved along ECI-frame axes.
344 * @param position body position expressed in meters (m) with respect ECI frame,
345 * resolved along ECI-frame axes.
346 * @param result instance where estimated body kinematics will be stored.
347 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
348 * are not ECI frame valid.
349 */
350 public void estimate(
351 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
352 final double vx, final double vy, final double vz,
353 final double oldVx, final double oldVy, final double oldVz,
354 final Point3D position, final BodyKinematics result) {
355 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position, result);
356 }
357
358 /**
359 * Estimates body kinematics (specific force applied to a body and its angular rates)..
360 *
361 * @param timeInterval time interval between epochs.
362 * @param c body-to-ECI-frame coordinate transformation.
363 * @param oldC previous body-to-ECI-frame coordinate transformation.
364 * @param vx x coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
365 * along ECI-frame axes.
366 * @param vy y coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
367 * along ECI-frame axes.
368 * @param vz z coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
369 * along ECI-frame axes.
370 * @param oldVx x coordinate of previous velocity of body frame expressed in meters per second (m/s) and
371 * resolved along ECI-frame axes.
372 * @param oldVy y coordinate of previous velocity of body frame expressed in meters per second (m/s) and
373 * resolved along ECI-frame axes.
374 * @param oldVz z coordinate of previous velocity of body frame expressed in meters per second (m/s) and
375 * resolved along ECI-frame axes.
376 * @param position body position expressed in meters (m) with respect ECI frame,
377 * resolved along ECI-frame axes.
378 * @param result instance where estimated body kinematics will be stored.
379 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
380 * are not ECI frame valid.
381 */
382 public void estimate(
383 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
384 final double vx, final double vy, final double vz,
385 final double oldVx, final double oldVy, final double oldVz,
386 final Point3D position, final BodyKinematics result) {
387 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position, 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 c body-to-ECI-frame coordinate transformation.
395 * @param oldC previous body-to-ECI-frame coordinate transformation.
396 * @param vx x coordinate of velocity of body frame with respect ECI frame, resolved
397 * along ECI-frame axes.
398 * @param vy y coordinate of velocity of body frame with respect ECI frame, resolved
399 * along ECI-frame axes.
400 * @param vz z coordinate of velocity of body frame with respect ECI frame, resolved
401 * along ECI-frame axes.
402 * @param oldVx x coordinate of previous velocity of body frame respect ECI frame,
403 * resolved along ECI-frame axes.
404 * @param oldVy y coordinate of previous velocity of body frame respect ECI frame,
405 * resolved along ECI-frame axes.
406 * @param oldVz z coordinate of previous velocity of body frame respect ECI frame,
407 * resolved along ECI-frame axes.
408 * @param position body position expressed in meters (m) with respect ECI frame,
409 * resolved along ECI-frame axes.
410 * @param result instance where estimated body kinematics will be stored.
411 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
412 * are not ECI frame valid.
413 */
414 public void estimate(
415 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
416 final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
417 final Point3D position, final BodyKinematics result) {
418 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position, result);
419 }
420
421 /**
422 * Estimates body kinematics (specific force applied to a body and its angular rates)..
423 *
424 * @param timeInterval time interval between epochs.
425 * @param c body-to-ECI-frame coordinate transformation.
426 * @param oldC previous body-to-ECI-frame coordinate transformation.
427 * @param vx x coordinate of velocity of body frame with respect ECI frame, resolved
428 * along ECI-frame axes.
429 * @param vy y coordinate of velocity of body frame with respect ECI frame, resolved
430 * along ECI-frame axes.
431 * @param vz z coordinate of velocity of body frame with respect ECI frame, resolved
432 * along ECI-frame axes.
433 * @param oldVx x coordinate of previous velocity of body frame respect ECI frame,
434 * resolved along ECI-frame axes.
435 * @param oldVy y coordinate of previous velocity of body frame respect ECI frame,
436 * resolved along ECI-frame axes.
437 * @param oldVz z coordinate of previous velocity of body frame respect ECI frame,
438 * resolved along ECI-frame axes.
439 * @param position body position expressed in meters (m) with respect ECI frame,
440 * resolved along ECI-frame axes.
441 * @param result instance where estimated body kinematics will be stored.
442 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
443 * are not ECI frame valid.
444 */
445 public void estimate(
446 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
447 final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
448 final Point3D position, final BodyKinematics result) {
449 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position, result);
450 }
451
452 /**
453 * Estimates body kinematics (specific force applied to a body and its angular rates)..
454 *
455 * @param timeInterval time interval between epochs expressed in seconds (s).
456 * @param c body-to-ECI-frame coordinate transformation.
457 * @param oldC previous body-to-ECI-frame coordinate transformation.
458 * @param vx x coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
459 * along ECI-frame axes.
460 * @param vy y coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
461 * along ECI-frame axes.
462 * @param vz z coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
463 * along ECI-frame axes.
464 * @param oldVx x coordinate of previous velocity of body frame expressed in meters per second (m/s) and
465 * resolved along ECI-frame axes.
466 * @param oldVy y coordinate of previous velocity of body frame expressed in meters per second (m/s) and
467 * resolved along ECI-frame axes.
468 * @param oldVz z coordinate of previous velocity of body frame expressed in meters per second (m/s) and
469 * resolved along ECI-frame axes.
470 * @param x cartesian x coordinate of body position expressed in meters (m) and resolved along
471 * ECI-frame axes.
472 * @param y cartesian y coordinate of body position expressed in meters (m) and resolved along
473 * ECI-frame axes.
474 * @param z cartesian z coordinate of body position expressed in meters (m) and resolved along
475 * ECI-frame axes.
476 * @return a new body kinematics instance.
477 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
478 * are not ECI frame valid.
479 */
480 public BodyKinematics estimateAndReturnNew(
481 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
482 final double vx, final double vy, final double vz,
483 final double oldVx, final double oldVy, final double oldVz,
484 final double x, final double y, final double z) {
485 return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z);
486 }
487
488 /**
489 * Estimates body kinematics (specific force applied to a body and its angular rates)..
490 *
491 * @param timeInterval time interval between epochs.
492 * @param c body-to-ECI-frame coordinate transformation.
493 * @param oldC previous body-to-ECI-frame coordinate transformation.
494 * @param vx x coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
495 * along ECI-frame axes.
496 * @param vy y coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
497 * along ECI-frame axes.
498 * @param vz z coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
499 * along ECI-frame axes.
500 * @param oldVx x coordinate of previous velocity of body frame expressed in meters per second (m/s) and
501 * resolved along ECI-frame axes.
502 * @param oldVy y coordinate of previous velocity of body frame expressed in meters per second (m/s) and
503 * resolved along ECI-frame axes.
504 * @param oldVz z coordinate of previous velocity of body frame expressed in meters per second (m/s) and
505 * resolved along ECI-frame axes.
506 * @param x cartesian x coordinate of body position expressed in meters (m) and resolved along
507 * ECI-frame axes.
508 * @param y cartesian y coordinate of body position expressed in meters (m) and resolved along
509 * ECI-frame axes.
510 * @param z cartesian z coordinate of body position expressed in meters (m) and resolved along
511 * ECI-frame axes.
512 * @return a new body kinematics instance.
513 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
514 * are not ECI frame valid.
515 */
516 public BodyKinematics estimateAndReturnNew(
517 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
518 final double vx, final double vy, final double vz,
519 final double oldVx, final double oldVy, final double oldVz,
520 final double x, final double y, final double z) {
521 return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z);
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 expressed in seconds (s).
528 * @param frame body ECI frame containing current position, velocity and
529 * body-to-ECI frame coordinate transformation.
530 * @param oldC previous body-to-ECI-frame coordinate transformation.
531 * @param oldVx x coordinate of previous velocity of body frame expressed in meters per second (m/s) and
532 * resolved along ECI-frame axes.
533 * @param oldVy y coordinate of previous velocity of body frame expressed in meters per second (m/s) and
534 * resolved along ECI-frame axes.
535 * @param oldVz z coordinate of previous velocity of body frame expressed in meters per second (m/s) and
536 * resolved along ECI-frame axes.
537 * @return a new body kinematics instance.
538 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
539 * are not ECI frame valid.
540 */
541 public BodyKinematics estimateAndReturnNew(
542 final double timeInterval, final ECIFrame frame, final CoordinateTransformation oldC,
543 final double oldVx, final double oldVy, final double oldVz) {
544 return estimateKinematicsAndReturnNew(timeInterval, frame, oldC, oldVx, oldVy, oldVz);
545 }
546
547 /**
548 * Estimates body kinematics (specific force applied to a body and its angular rates).
549 *
550 * @param timeInterval time interval between epochs.
551 * @param frame body ECI frame containing current position, velocity and
552 * body-to-ECI frame coordinate transformation.
553 * @param oldC previous body-to-ECI-frame coordinate transformation.
554 * @param oldVx x coordinate of previous velocity of body frame expressed in meters per second (m/s) and
555 * resolved along ECI-frame axes.
556 * @param oldVy y coordinate of previous velocity of body frame expressed in meters per second (m/s) and
557 * resolved along ECI-frame axes.
558 * @param oldVz z coordinate of previous velocity of body frame expressed in meters per second (m/s) and
559 * resolved along ECI-frame axes.
560 * @return a new body kinematics instance.
561 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
562 * are not ECI frame valid.
563 */
564 public BodyKinematics estimateAndReturnNew(
565 final Time timeInterval, final ECIFrame frame, final CoordinateTransformation oldC,
566 final double oldVx, final double oldVy, final double oldVz) {
567 return estimateKinematicsAndReturnNew(timeInterval, frame, oldC, oldVx, oldVy, oldVz);
568 }
569
570 /**
571 * Estimates body kinematics (specific force applied to a body and its angular rates).
572 *
573 * @param timeInterval time interval between epochs expressed in seconds (s).
574 * @param frame body ECI frame containing current position, velocity and
575 * body-to-ECI frame coordinate transformation.
576 * @param oldFrame body ECI frame containing previous position, velocity and
577 * body-to-ECI frame coordinate transformation. Notice that
578 * previous position contained in this frame is ignored.
579 * @return a new body kinematics instance.
580 * @throws IllegalArgumentException if provided time interval is negative.
581 */
582 public BodyKinematics estimateAndReturnNew(
583 final double timeInterval, final ECIFrame frame, final ECIFrame oldFrame) {
584 return estimateKinematicsAndReturnNew(timeInterval, frame, oldFrame);
585 }
586
587 /**
588 * Estimates body kinematics (specific force applied to a body and its angular rates).
589 *
590 * @param timeInterval time interval between epochs.
591 * @param frame body ECI frame containing current position, velocity and
592 * body-to-ECI frame coordinate transformation.
593 * @param oldFrame body ECI frame containing previous position, velocity and
594 * body-to-ECI frame coordinate transformation. Notice that
595 * previous position contained in this frame is ignored.
596 * @return a new body kinematics instance.
597 * @throws IllegalArgumentException if provided time interval is negative.
598 */
599 public BodyKinematics estimateAndReturnNew(
600 final Time timeInterval, final ECIFrame frame, final ECIFrame oldFrame) {
601 return estimateKinematicsAndReturnNew(timeInterval, frame, oldFrame);
602 }
603
604 /**
605 * Estimates body kinematics (specific force applied to a body and its angular rates)..
606 *
607 * @param timeInterval time interval between epochs expressed in seconds (s).
608 * @param c body-to-ECI-frame coordinate transformation.
609 * @param oldC previous body-to-ECI-frame coordinate transformation.
610 * @param vx x coordinate of velocity of body frame with respect ECI frame, resolved
611 * along ECI-frame axes.
612 * @param vy y coordinate of velocity of body frame with respect ECI frame, resolved
613 * along ECI-frame axes.
614 * @param vz z coordinate of velocity of body frame with respect ECI frame, resolved
615 * along ECI-frame axes.
616 * @param oldVx x coordinate of previous velocity of body frame with respect ECI frame,
617 * resolved along ECI-frame axes.
618 * @param oldVy y coordinate of previous velocity of body frame with respect ECI frame,
619 * resolved along ECI-frame axes.
620 * @param oldVz z coordinate of previous velocity of body frame with respect ECI frame,
621 * resolved along ECI-frame axes.
622 * @param x cartesian x coordinate of body position expressed in meters (m) and resolved along
623 * ECI-frame axes.
624 * @param y cartesian y coordinate of body position expressed in meters (m) and resolved along
625 * ECI-frame axes.
626 * @param z cartesian z coordinate of body position expressed in meters (m) and resolved along
627 * ECI-frame axes.
628 * @return a new body kinematics instance.
629 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
630 * are not ECI frame valid.
631 */
632 public BodyKinematics estimateAndReturnNew(
633 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
634 final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
635 final double x, final double y, final double z) {
636 return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z);
637 }
638
639 /**
640 * Estimates body kinematics (specific force applied to a body and its angular rates)..
641 *
642 * @param timeInterval time interval between epochs.
643 * @param c body-to-ECI-frame coordinate transformation.
644 * @param oldC previous body-to-ECI-frame coordinate transformation.
645 * @param vx x coordinate of velocity of body frame with respect ECI frame, resolved
646 * along ECI-frame axes.
647 * @param vy y coordinate of velocity of body frame with respect ECI frame, resolved
648 * along ECI-frame axes.
649 * @param vz z coordinate of velocity of body frame with respect ECI frame, resolved
650 * along ECI-frame axes.
651 * @param oldVx x coordinate of previous velocity of body frame with respect ECI frame,
652 * resolved along ECI-frame axes.
653 * @param oldVy y coordinate of previous velocity of body frame with respect ECI frame,
654 * resolved along ECI-frame axes.
655 * @param oldVz z coordinate of previous velocity of body frame with respect ECI frame,
656 * resolved along ECI-frame axes.
657 * @param x cartesian x coordinate of body position expressed in meters (m) and resolved along
658 * ECI-frame axes.
659 * @param y cartesian y coordinate of body position expressed in meters (m) and resolved along
660 * ECI-frame axes.
661 * @param z cartesian z coordinate of body position expressed in meters (m) and resolved along
662 * ECI-frame axes.
663 * @return a new body kinematics instance.
664 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
665 * are not ECI frame valid.
666 */
667 public BodyKinematics estimateAndReturnNew(
668 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
669 final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
670 final double x, final double y, final double z) {
671 return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z);
672 }
673
674 /**
675 * Estimates body kinematics (specific force applied to a body and its angular rates).
676 *
677 * @param timeInterval time interval between epochs expressed in seconds (s).
678 * @param frame body ECI frame containing current position, velocity and
679 * body-to-ECI frame coordinate transformation.
680 * @param oldC previous body-to-ECI-frame coordinate transformation.
681 * @param oldVx x coordinate of previous velocity of body frame with respect ECI frame,
682 * resolved along ECI-frame axes.
683 * @param oldVy y coordinate of previous velocity of body frame with respect ECI frame,
684 * resolved along ECI-frame axes.
685 * @param oldVz z coordinate of previous velocity of body frame with respect ECI frame,
686 * resolved along ECI-frame axes.
687 * @return a new body kinematics instance.
688 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
689 * are not ECI frame valid.
690 */
691 public BodyKinematics estimateAndReturnNew(
692 final double timeInterval, final ECIFrame frame, final CoordinateTransformation oldC,
693 final Speed oldVx, final Speed oldVy, final Speed oldVz) {
694 return estimateKinematicsAndReturnNew(timeInterval, frame, oldC, oldVx, oldVy, oldVz);
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 frame body ECI frame containing current position, velocity and
702 * body-to-ECI frame coordinate transformation.
703 * @param oldC previous body-to-ECI-frame coordinate transformation.
704 * @param oldVx x coordinate of previous velocity of body frame with respect ECI frame,
705 * resolved along ECI-frame axes.
706 * @param oldVy y coordinate of previous velocity of body frame with respect ECI frame,
707 * resolved along ECI-frame axes.
708 * @param oldVz z coordinate of previous velocity of body frame with respect ECI frame,
709 * resolved along ECI-frame axes.
710 * @return a new body kinematics instance.
711 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
712 * are not ECI frame valid.
713 */
714 public BodyKinematics estimateAndReturnNew(
715 final Time timeInterval, final ECIFrame frame, final CoordinateTransformation oldC,
716 final Speed oldVx, final Speed oldVy, final Speed oldVz) {
717 return estimateKinematicsAndReturnNew(timeInterval, frame, oldC, oldVx, oldVy, oldVz);
718 }
719
720 /**
721 * Estimates body kinematics (specific force applied to a body and its angular rates)..
722 *
723 * @param timeInterval time interval between epochs expressed in seconds (s).
724 * @param c body-to-ECI-frame coordinate transformation.
725 * @param oldC previous body-to-ECI-frame coordinate transformation.
726 * @param vx x coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
727 * along ECI-frame axes.
728 * @param vy y coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
729 * along ECI-frame axes.
730 * @param vz z coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
731 * along ECI-frame axes.
732 * @param oldVx x coordinate of previous velocity of body frame expressed in meters per second (m/s) and
733 * resolved along ECI-frame axes.
734 * @param oldVy y coordinate of previous velocity of body frame expressed in meters per second (m/s) and
735 * resolved along ECI-frame axes.
736 * @param oldVz z coordinate of previous velocity of body frame expressed in meters per second (m/s) and
737 * resolved along ECI-frame axes.
738 * @param position body position expressed in meters (m) with respect ECI frame,
739 * resolved along ECI-frame axes.
740 * @return a new body kinematics instance.
741 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
742 * are not ECI frame valid.
743 */
744 public BodyKinematics estimateAndReturnNew(
745 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
746 final double vx, final double vy, final double vz,
747 final double oldVx, final double oldVy, final double oldVz, final Point3D position) {
748 return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position);
749 }
750
751 /**
752 * Estimates body kinematics (specific force applied to a body and its angular rates)..
753 *
754 * @param timeInterval time interval between epochs.
755 * @param c body-to-ECI-frame coordinate transformation.
756 * @param oldC previous body-to-ECI-frame coordinate transformation.
757 * @param vx x coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
758 * along ECI-frame axes.
759 * @param vy y coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
760 * along ECI-frame axes.
761 * @param vz z coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
762 * along ECI-frame axes.
763 * @param oldVx x coordinate of previous velocity of body frame expressed in meters per second (m/s) and
764 * resolved along ECI-frame axes.
765 * @param oldVy y coordinate of previous velocity of body frame expressed in meters per second (m/s) and
766 * resolved along ECI-frame axes.
767 * @param oldVz z coordinate of previous velocity of body frame expressed in meters per second (m/s) and
768 * resolved along ECI-frame axes.
769 * @param position body position expressed in meters (m) with respect ECI frame,
770 * resolved along ECI-frame axes.
771 * @return a new body kinematics instance.
772 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
773 * are not ECI frame valid.
774 */
775 public BodyKinematics estimateAndReturnNew(
776 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
777 final double vx, final double vy, final double vz,
778 final double oldVx, final double oldVy, final double oldVz, final Point3D position) {
779 return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position);
780 }
781
782 /**
783 * Estimates body kinematics (specific force applied to a body and its angular rates)..
784 *
785 * @param timeInterval time interval between epochs expressed in seconds (s).
786 * @param c body-to-ECI-frame coordinate transformation.
787 * @param oldC previous body-to-ECI-frame coordinate transformation.
788 * @param vx x coordinate of velocity of body frame with respect ECI frame, resolved
789 * along ECI-frame axes.
790 * @param vy y coordinate of velocity of body frame with respect ECI frame, resolved
791 * along ECI-frame axes.
792 * @param vz z coordinate of velocity of body frame with respect ECI frame, resolved
793 * along ECI-frame axes.
794 * @param oldVx x coordinate of previous velocity of body frame respect ECI frame,
795 * resolved along ECI-frame axes.
796 * @param oldVy y coordinate of previous velocity of body frame respect ECI frame,
797 * resolved along ECI-frame axes.
798 * @param oldVz z coordinate of previous velocity of body frame respect ECI frame,
799 * resolved along ECI-frame axes.
800 * @param position body position expressed in meters (m) with respect ECI frame,
801 * resolved along ECI-frame axes.
802 * @return a new body kinematics instance.
803 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
804 * are not ECI frame valid.
805 */
806 public BodyKinematics estimateAndReturnNew(
807 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
808 final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
809 final Point3D position) {
810 return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position);
811 }
812
813 /**
814 * Estimates body kinematics (specific force applied to a body and its angular rates)..
815 *
816 * @param timeInterval time interval between epochs.
817 * @param c body-to-ECI-frame coordinate transformation.
818 * @param oldC previous body-to-ECI-frame coordinate transformation.
819 * @param vx x coordinate of velocity of body frame with respect ECI frame, resolved
820 * along ECI-frame axes.
821 * @param vy y coordinate of velocity of body frame with respect ECI frame, resolved
822 * along ECI-frame axes.
823 * @param vz z coordinate of velocity of body frame with respect ECI frame, resolved
824 * along ECI-frame axes.
825 * @param oldVx x coordinate of previous velocity of body frame respect ECI frame,
826 * resolved along ECI-frame axes.
827 * @param oldVy y coordinate of previous velocity of body frame respect ECI frame,
828 * resolved along ECI-frame axes.
829 * @param oldVz z coordinate of previous velocity of body frame respect ECI frame,
830 * resolved along ECI-frame axes.
831 * @param position body position expressed in meters (m) with respect ECI frame,
832 * resolved along ECI-frame axes.
833 * @return a new body kinematics instance.
834 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
835 * are not ECI frame valid.
836 */
837 public BodyKinematics estimateAndReturnNew(
838 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
839 final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
840 final Point3D position) {
841 return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position);
842 }
843
844 /**
845 * Estimates body kinematics (specific force applied to a body and its angular rates).
846 *
847 * @param timeInterval time interval between epochs expressed in seconds (s).
848 * @param c body-to-ECI-frame coordinate transformation.
849 * @param oldC previous body-to-ECI-frame coordinate transformation.
850 * @param vx x coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
851 * along ECI-frame axes.
852 * @param vy y coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
853 * along ECI-frame axes.
854 * @param vz z coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
855 * along ECI-frame axes.
856 * @param oldVx x coordinate of previous velocity of body frame expressed in meters per second (m/s) and
857 * resolved along ECI-frame axes.
858 * @param oldVy y coordinate of previous velocity of body frame expressed in meters per second (m/s) and
859 * resolved along ECI-frame axes.
860 * @param oldVz z coordinate of previous velocity of body frame expressed in meters per second (m/s) and
861 * resolved along ECI-frame axes.
862 * @param x cartesian x coordinate of body position expressed in meters (m) and resolved along
863 * ECI-frame axes.
864 * @param y cartesian y coordinate of body position expressed in meters (m) and resolved along
865 * ECI-frame axes.
866 * @param z cartesian z coordinate of body position expressed in meters (m) and resolved along
867 * ECI-frame axes.
868 * @param result instance where estimated body kinematics will be stored.
869 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
870 * are not ECI frame valid.
871 */
872 public static void estimateKinematics(
873 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
874 final double vx, final double vy, final double vz,
875 final double oldVx, final double oldVy, final double oldVz,
876 final double x, final double y, final double z, final BodyKinematics result) {
877 if (timeInterval < 0.0 || !ECIFrame.isValidCoordinateTransformation(c)
878 || !ECIFrame.isValidCoordinateTransformation(oldC)) {
879 throw new IllegalArgumentException();
880 }
881
882 if (timeInterval > 0.0) {
883 try {
884 // Obtain coordinate transformation matrix from the old attitude to the new
885 // initially c contains body-to-ECI transformation
886 final var cib = c.getMatrix();
887 // we transpose it to obtain the inverse
888 cib.transpose();
889
890 final var oldCbi = oldC.getMatrix();
891 final var cOldNew = cib.multiplyAndReturnNew(oldCbi);
892
893 // Calculate the approximate angular rate
894 var alphaIbbX = 0.5 * (cOldNew.getElementAt(1, 2)
895 - cOldNew.getElementAt(2, 1));
896 var alphaIbbY = 0.5 * (cOldNew.getElementAt(2, 0)
897 - cOldNew.getElementAt(0, 2));
898 var alphaIbbZ = 0.5 * (cOldNew.getElementAt(0, 1)
899 - cOldNew.getElementAt(1, 0));
900
901 // Calculate and apply the scaling factor
902 final var temp = Math.acos(0.5 * (cOldNew.getElementAt(0, 0)
903 + cOldNew.getElementAt(1, 1)
904 + cOldNew.getElementAt(2, 2) - 1.0));
905 if (temp > SCALING_THRESHOLD) {
906 // Scaling is 1 if temp is less than this
907 final var scaling = temp / Math.sin(temp);
908 alphaIbbX *= scaling;
909 alphaIbbY *= scaling;
910 alphaIbbZ *= scaling;
911 }
912
913 // Calculate the angular rate
914 final var angularRateX = alphaIbbX / timeInterval;
915 final var angularRateY = alphaIbbY / timeInterval;
916 final var angularRateZ = alphaIbbZ / timeInterval;
917
918 // Calculate the specific force resolved about ECI-frame axes
919 // From (5.18) and (5.20)
920 final var vIbi = new Matrix(ROWS, 1);
921 vIbi.setElementAtIndex(0, vx);
922 vIbi.setElementAtIndex(1, vy);
923 vIbi.setElementAtIndex(2, vz);
924
925 final var oldVibi = new Matrix(ROWS, 1);
926 oldVibi.setElementAtIndex(0, oldVx);
927 oldVibi.setElementAtIndex(1, oldVy);
928 oldVibi.setElementAtIndex(2, oldVz);
929
930 final var gravitation = ECIGravitationEstimator.estimateGravitationAndReturnNew(x, y, z);
931 final var g = gravitation.asMatrix();
932
933 // fIbi = ((vIbi - oldVibi) / timeInterval) - g
934 vIbi.subtract(oldVibi);
935 vIbi.multiplyByScalar(1.0 / timeInterval);
936 vIbi.subtract(g);
937 // vIbi now contains specific force resolved about ECI-frame axes
938
939 // Calculate the average body-to-ECI-frame coordinate transformation
940 // matrix over the update interval using (5.84)
941 final var alphaNorm = Math.sqrt(alphaIbbX * alphaIbbX + alphaIbbY * alphaIbbY + alphaIbbZ * alphaIbbZ);
942 final var alphaSkew = Utils.skewMatrix(new double[]{alphaIbbX, alphaIbbY, alphaIbbZ});
943
944 if (alphaNorm > ALPHA_THRESHOLD) {
945 final var alphaNorm2 = alphaNorm * alphaNorm;
946 final var value1 = (1.0 - Math.cos(alphaNorm)) / alphaNorm2;
947 final var value2 = (1.0 - Math.sin(alphaNorm) / alphaNorm) / alphaNorm2;
948 final var tmp1 = alphaSkew.multiplyByScalarAndReturnNew(value1);
949 final var tmp2 = alphaSkew.multiplyByScalarAndReturnNew(value2);
950 tmp2.multiply(alphaSkew);
951
952 final var tmp3 = Matrix.identity(ROWS, ROWS);
953 tmp3.add(tmp1);
954 tmp3.add(tmp2);
955
956 oldCbi.multiply(tmp3);
957 }
958
959 // oldCbi now contains the average body-to-ECI-frame coordinate transformation
960
961 // Transform specific force to body-frame resolving axes using (5.81)
962 final var invAveCbi = Utils.inverse(oldCbi);
963 invAveCbi.multiply(vIbi);
964
965 final var specificForceX = invAveCbi.getElementAtIndex(0);
966 final var specificForceY = invAveCbi.getElementAtIndex(1);
967 final var specificForceZ = invAveCbi.getElementAtIndex(2);
968
969 // save result data
970 result.setSpecificForceCoordinates(specificForceX, specificForceY, specificForceZ);
971 result.setAngularRateCoordinates(angularRateX, angularRateY, angularRateZ);
972
973 } catch (final AlgebraException ignore) {
974 // never happens
975 }
976 } else {
977 // If time interval is zero, set angular rate and specific force to zero
978 result.setSpecificForceCoordinates(0.0, 0.0, 0.0);
979 result.setAngularRateCoordinates(0.0, 0.0, 0.0);
980 }
981 }
982
983 /**
984 * Estimates body kinematics (specific force applied to a body and its angular rates)..
985 *
986 * @param timeInterval time interval between epochs.
987 * @param c body-to-ECI-frame coordinate transformation.
988 * @param oldC previous body-to-ECI-frame coordinate transformation.
989 * @param vx x coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
990 * along ECI-frame axes.
991 * @param vy y coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
992 * along ECI-frame axes.
993 * @param vz z coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
994 * along ECI-frame axes.
995 * @param oldVx x coordinate of previous velocity of body frame expressed in meters per second (m/s) and
996 * resolved along ECI-frame axes.
997 * @param oldVy y coordinate of previous velocity of body frame expressed in meters per second (m/s) and
998 * resolved along ECI-frame axes.
999 * @param oldVz z coordinate of previous velocity of body frame expressed in meters per second (m/s) and
1000 * resolved along ECI-frame axes.
1001 * @param x cartesian x coordinate of body position expressed in meters (m) and resolved along
1002 * ECI-frame axes.
1003 * @param y cartesian y coordinate of body position expressed in meters (m) and resolved along
1004 * ECI-frame axes.
1005 * @param z cartesian z coordinate of body position expressed in meters (m) and resolved along
1006 * ECI-frame axes.
1007 * @param result instance where estimated body kinematics will be stored.
1008 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
1009 * are not ECI frame valid.
1010 */
1011 public static void estimateKinematics(
1012 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
1013 final double vx, final double vy, final double vz,
1014 final double oldVx, final double oldVy, final double oldVz,
1015 final double x, final double y, final double z, final BodyKinematics result) {
1016 estimateKinematics(
1017 TimeConverter.convert(timeInterval.getValue().doubleValue(), timeInterval.getUnit(), TimeUnit.SECOND),
1018 c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z, result);
1019 }
1020
1021 /**
1022 * Estimates body kinematics (specific force applied to a body and its angular rates).
1023 *
1024 * @param timeInterval time interval between epochs expressed in seconds (s).
1025 * @param frame body ECI frame containing current position, velocity and
1026 * body-to-ECI frame coordinate transformation.
1027 * @param oldC previous body-to-ECI-frame coordinate transformation.
1028 * @param oldVx x coordinate of previous velocity of body frame expressed in meters per second (m/s) and
1029 * resolved along ECI-frame axes.
1030 * @param oldVy y coordinate of previous velocity of body frame expressed in meters per second (m/s) and
1031 * resolved along ECI-frame axes.
1032 * @param oldVz z coordinate of previous velocity of body frame expressed in meters per second (m/s) and
1033 * resolved along ECI-frame axes.
1034 * @param result instance where estimated body kinematics will be stored.
1035 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
1036 * are not ECI frame valid.
1037 */
1038 public static void estimateKinematics(
1039 final double timeInterval, final ECIFrame frame, final CoordinateTransformation oldC,
1040 final double oldVx, final double oldVy, final double oldVz, final BodyKinematics result) {
1041 estimateKinematics(timeInterval, frame.getCoordinateTransformation(), oldC,
1042 frame.getVx(), frame.getVy(), frame.getVz(), oldVx, oldVy, oldVz,
1043 frame.getX(), frame.getY(), frame.getZ(), result);
1044 }
1045
1046 /**
1047 * Estimates body kinematics (specific force applied to a body and its angular rates).
1048 *
1049 * @param timeInterval time interval between epochs.
1050 * @param frame body ECI frame containing current position, velocity and
1051 * body-to-ECI frame coordinate transformation.
1052 * @param oldC previous body-to-ECI-frame coordinate transformation.
1053 * @param oldVx x coordinate of previous velocity of body frame expressed in meters per second (m/s) and
1054 * resolved along ECI-frame axes.
1055 * @param oldVy y coordinate of previous velocity of body frame expressed in meters per second (m/s) and
1056 * resolved along ECI-frame axes.
1057 * @param oldVz z coordinate of previous velocity of body frame expressed in meters per second (m/s) and
1058 * resolved along ECI-frame axes.
1059 * @param result instance where estimated body kinematics will be stored.
1060 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
1061 * are not ECI frame valid.
1062 */
1063 public static void estimateKinematics(
1064 final Time timeInterval, final ECIFrame frame, final CoordinateTransformation oldC,
1065 final double oldVx, final double oldVy, final double oldVz, final BodyKinematics result) {
1066 estimateKinematics(
1067 TimeConverter.convert(timeInterval.getValue().doubleValue(), timeInterval.getUnit(), TimeUnit.SECOND),
1068 frame.getCoordinateTransformation(), oldC, frame.getVx(), frame.getVy(), frame.getVz(),
1069 oldVx, oldVy, oldVz, frame.getX(), frame.getY(), frame.getZ(), result);
1070 }
1071
1072 /**
1073 * Estimates body kinematics (specific force applied to a body and its angular rates).
1074 *
1075 * @param timeInterval time interval between epochs expressed in seconds (s).
1076 * @param frame body ECI frame containing current position, velocity and
1077 * body-to-ECI frame coordinate transformation.
1078 * @param oldFrame body ECI frame containing previous position, velocity and
1079 * body-to-ECI frame coordinate transformation. Notice that
1080 * previous position contained in this frame is ignored.
1081 * @param result instance where body kinematics estimation will be stored.
1082 * @throws IllegalArgumentException if provided time interval is negative.
1083 */
1084 public static void estimateKinematics(
1085 final double timeInterval, final ECIFrame frame, final ECIFrame oldFrame, final BodyKinematics result) {
1086 estimateKinematics(timeInterval, frame, oldFrame.getCoordinateTransformation(),
1087 oldFrame.getVx(), oldFrame.getVy(), oldFrame.getVz(), result);
1088 }
1089
1090 /**
1091 * Estimates body kinematics (specific force applied to a body and its angular rates).
1092 *
1093 * @param timeInterval time interval between epochs.
1094 * @param frame body ECI frame containing current position, velocity and
1095 * body-to-ECI frame coordinate transformation.
1096 * @param oldFrame body ECI frame containing previous position, velocity and
1097 * body-to-ECI frame coordinate transformation. Notice that
1098 * previous position contained in this frame is ignored.
1099 * @param result instance where body kinematics estimation will be stored.
1100 * @throws IllegalArgumentException if provided time interval is negative.
1101 */
1102 public static void estimateKinematics(
1103 final Time timeInterval, final ECIFrame frame, final ECIFrame oldFrame, final BodyKinematics result) {
1104 estimateKinematics(timeInterval, frame, oldFrame.getCoordinateTransformation(),
1105 oldFrame.getVx(), oldFrame.getVy(), oldFrame.getVz(), result);
1106 }
1107
1108 /**
1109 * Estimates body kinematics (specific force applied to a body and its angular rates)..
1110 *
1111 * @param timeInterval time interval between epochs expressed in seconds (s).
1112 * @param c body-to-ECI-frame coordinate transformation.
1113 * @param oldC previous body-to-ECI-frame coordinate transformation.
1114 * @param vx x coordinate of velocity of body frame with respect ECI frame, resolved
1115 * along ECI-frame axes.
1116 * @param vy y coordinate of velocity of body frame with respect ECI frame, resolved
1117 * along ECI-frame axes.
1118 * @param vz z coordinate of velocity of body frame with respect ECI frame, resolved
1119 * along ECI-frame axes.
1120 * @param oldVx x coordinate of previous velocity of body frame with respect ECI frame,
1121 * resolved along ECI-frame axes.
1122 * @param oldVy y coordinate of previous velocity of body frame with respect ECI frame,
1123 * resolved along ECI-frame axes.
1124 * @param oldVz z coordinate of previous velocity of body frame with respect ECI frame,
1125 * resolved along ECI-frame axes.
1126 * @param x cartesian x coordinate of body position expressed in meters (m) and resolved along
1127 * ECI-frame axes.
1128 * @param y cartesian y coordinate of body position expressed in meters (m) and resolved along
1129 * ECI-frame axes.
1130 * @param z cartesian z coordinate of body position expressed in meters (m) and resolved along
1131 * ECI-frame axes.
1132 * @param result instance where estimated body kinematics will be stored.
1133 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
1134 * are not ECI frame valid.
1135 */
1136 public static void estimateKinematics(
1137 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
1138 final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
1139 final double x, final double y, final double z, final BodyKinematics result) {
1140 estimateKinematics(timeInterval, c, oldC,
1141 SpeedConverter.convert(vx.getValue().doubleValue(), vx.getUnit(), SpeedUnit.METERS_PER_SECOND),
1142 SpeedConverter.convert(vy.getValue().doubleValue(), vy.getUnit(), SpeedUnit.METERS_PER_SECOND),
1143 SpeedConverter.convert(vz.getValue().doubleValue(), vz.getUnit(), SpeedUnit.METERS_PER_SECOND),
1144 SpeedConverter.convert(oldVx.getValue().doubleValue(), oldVx.getUnit(), SpeedUnit.METERS_PER_SECOND),
1145 SpeedConverter.convert(oldVy.getValue().doubleValue(), oldVy.getUnit(), SpeedUnit.METERS_PER_SECOND),
1146 SpeedConverter.convert(oldVz.getValue().doubleValue(), oldVz.getUnit(), SpeedUnit.METERS_PER_SECOND),
1147 x, y, z, result);
1148 }
1149
1150 /**
1151 * Estimates body kinematics (specific force applied to a body and its angular rates)..
1152 *
1153 * @param timeInterval time interval between epochs.
1154 * @param c body-to-ECI-frame coordinate transformation.
1155 * @param oldC previous body-to-ECI-frame coordinate transformation.
1156 * @param vx x coordinate of velocity of body frame with respect ECI frame, resolved
1157 * along ECI-frame axes.
1158 * @param vy y coordinate of velocity of body frame with respect ECI frame, resolved
1159 * along ECI-frame axes.
1160 * @param vz z coordinate of velocity of body frame with respect ECI frame, resolved
1161 * along ECI-frame axes.
1162 * @param oldVx x coordinate of previous velocity of body frame with respect ECI frame,
1163 * resolved along ECI-frame axes.
1164 * @param oldVy y coordinate of previous velocity of body frame with respect ECI frame,
1165 * resolved along ECI-frame axes.
1166 * @param oldVz z coordinate of previous velocity of body frame with respect ECI frame,
1167 * resolved along ECI-frame axes.
1168 * @param x cartesian x coordinate of body position expressed in meters (m) and resolved along
1169 * ECI-frame axes.
1170 * @param y cartesian y coordinate of body position expressed in meters (m) and resolved along
1171 * ECI-frame axes.
1172 * @param z cartesian z coordinate of body position expressed in meters (m) and resolved along
1173 * ECI-frame axes.
1174 * @param result instance where estimated body kinematics will be stored.
1175 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
1176 * are not ECI frame valid.
1177 */
1178 public static void estimateKinematics(
1179 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
1180 final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
1181 final double x, final double y, final double z, final BodyKinematics result) {
1182 estimateKinematics(timeInterval, c, oldC,
1183 SpeedConverter.convert(vx.getValue().doubleValue(), vx.getUnit(), SpeedUnit.METERS_PER_SECOND),
1184 SpeedConverter.convert(vy.getValue().doubleValue(), vy.getUnit(), SpeedUnit.METERS_PER_SECOND),
1185 SpeedConverter.convert(vz.getValue().doubleValue(), vz.getUnit(), SpeedUnit.METERS_PER_SECOND),
1186 SpeedConverter.convert(oldVx.getValue().doubleValue(), oldVx.getUnit(), SpeedUnit.METERS_PER_SECOND),
1187 SpeedConverter.convert(oldVy.getValue().doubleValue(), oldVy.getUnit(), SpeedUnit.METERS_PER_SECOND),
1188 SpeedConverter.convert(oldVz.getValue().doubleValue(), oldVz.getUnit(), SpeedUnit.METERS_PER_SECOND),
1189 x, y, z, result);
1190 }
1191
1192 /**
1193 * Estimates body kinematics (specific force applied to a body and its angular rates).
1194 *
1195 * @param timeInterval time interval between epochs expressed in seconds (s).
1196 * @param frame body ECI frame containing current position, velocity and
1197 * body-to-ECI frame coordinate transformation.
1198 * @param oldC previous body-to-ECI-frame coordinate transformation.
1199 * @param oldVx x coordinate of previous velocity of body frame with respect ECI frame,
1200 * resolved along ECI-frame axes.
1201 * @param oldVy y coordinate of previous velocity of body frame with respect ECI frame,
1202 * resolved along ECI-frame axes.
1203 * @param oldVz z coordinate of previous velocity of body frame with respect ECI frame,
1204 * resolved along ECI-frame axes.
1205 * @param result instance where estimated body kinematics will be stored.
1206 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
1207 * are not ECI frame valid.
1208 */
1209 public static void estimateKinematics(
1210 final double timeInterval, final ECIFrame frame, final CoordinateTransformation oldC,
1211 final Speed oldVx, final Speed oldVy, final Speed oldVz, final BodyKinematics result) {
1212 estimateKinematics(timeInterval, frame, oldC,
1213 SpeedConverter.convert(oldVx.getValue().doubleValue(), oldVx.getUnit(), SpeedUnit.METERS_PER_SECOND),
1214 SpeedConverter.convert(oldVy.getValue().doubleValue(), oldVy.getUnit(), SpeedUnit.METERS_PER_SECOND),
1215 SpeedConverter.convert(oldVz.getValue().doubleValue(), oldVz.getUnit(), SpeedUnit.METERS_PER_SECOND),
1216 result);
1217 }
1218
1219 /**
1220 * Estimates body kinematics (specific force applied to a body and its angular rates).
1221 *
1222 * @param timeInterval time interval between epochs.
1223 * @param frame body ECI frame containing current position, velocity and
1224 * body-to-ECI frame coordinate transformation.
1225 * @param oldC previous body-to-ECI-frame coordinate transformation.
1226 * @param oldVx x coordinate of previous velocity of body frame with respect ECI frame,
1227 * resolved along ECI-frame axes.
1228 * @param oldVy y coordinate of previous velocity of body frame with respect ECI frame,
1229 * resolved along ECI-frame axes.
1230 * @param oldVz z coordinate of previous velocity of body frame with respect ECI frame,
1231 * resolved along ECI-frame axes.
1232 * @param result instance where estimated body kinematics will be stored.
1233 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
1234 * are not ECI frame valid.
1235 */
1236 public static void estimateKinematics(
1237 final Time timeInterval, final ECIFrame frame, final CoordinateTransformation oldC,
1238 final Speed oldVx, final Speed oldVy, final Speed oldVz, final BodyKinematics result) {
1239 estimateKinematics(timeInterval, frame, oldC,
1240 SpeedConverter.convert(oldVx.getValue().doubleValue(), oldVx.getUnit(), SpeedUnit.METERS_PER_SECOND),
1241 SpeedConverter.convert(oldVy.getValue().doubleValue(), oldVy.getUnit(), SpeedUnit.METERS_PER_SECOND),
1242 SpeedConverter.convert(oldVz.getValue().doubleValue(), oldVz.getUnit(), SpeedUnit.METERS_PER_SECOND),
1243 result);
1244 }
1245
1246 /**
1247 * Estimates body kinematics (specific force applied to a body and its angular rates)..
1248 *
1249 * @param timeInterval time interval between epochs expressed in seconds (s).
1250 * @param c body-to-ECI-frame coordinate transformation.
1251 * @param oldC previous body-to-ECI-frame coordinate transformation.
1252 * @param vx x coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
1253 * along ECI-frame axes.
1254 * @param vy y coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
1255 * along ECI-frame axes.
1256 * @param vz z coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
1257 * along ECI-frame axes.
1258 * @param oldVx x coordinate of previous velocity of body frame expressed in meters per second (m/s) and
1259 * resolved along ECI-frame axes.
1260 * @param oldVy y coordinate of previous velocity of body frame expressed in meters per second (m/s) and
1261 * resolved along ECI-frame axes.
1262 * @param oldVz z coordinate of previous velocity of body frame expressed in meters per second (m/s) and
1263 * resolved along ECI-frame axes.
1264 * @param position body position expressed in meters (m) with respect ECI frame,
1265 * resolved along ECI-frame axes.
1266 * @param result instance where estimated body kinematics will be stored.
1267 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
1268 * are not ECI frame valid.
1269 */
1270 public static void estimateKinematics(
1271 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
1272 final double vx, final double vy, final double vz,
1273 final double oldVx, final double oldVy, final double oldVz, final Point3D position,
1274 final BodyKinematics result) {
1275 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz,
1276 position.getInhomX(), position.getInhomY(), position.getInhomZ(), result);
1277 }
1278
1279 /**
1280 * Estimates body kinematics (specific force applied to a body and its angular rates)..
1281 *
1282 * @param timeInterval time interval between epochs.
1283 * @param c body-to-ECI-frame coordinate transformation.
1284 * @param oldC previous body-to-ECI-frame coordinate transformation.
1285 * @param vx x coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
1286 * along ECI-frame axes.
1287 * @param vy y coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
1288 * along ECI-frame axes.
1289 * @param vz z coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
1290 * along ECI-frame axes.
1291 * @param oldVx x coordinate of previous velocity of body frame expressed in meters per second (m/s) and
1292 * resolved along ECI-frame axes.
1293 * @param oldVy y coordinate of previous velocity of body frame expressed in meters per second (m/s) and
1294 * resolved along ECI-frame axes.
1295 * @param oldVz z coordinate of previous velocity of body frame expressed in meters per second (m/s) and
1296 * resolved along ECI-frame axes.
1297 * @param position body position expressed in meters (m) with respect ECI frame,
1298 * resolved along ECI-frame axes.
1299 * @param result instance where estimated body kinematics will be stored.
1300 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
1301 * are not ECI frame valid.
1302 */
1303 public static void estimateKinematics(
1304 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
1305 final double vx, final double vy, final double vz,
1306 final double oldVx, final double oldVy, final double oldVz, final Point3D position,
1307 final BodyKinematics result) {
1308 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz,
1309 position.getInhomX(), position.getInhomY(), position.getInhomZ(), result);
1310 }
1311
1312 /**
1313 * Estimates body kinematics (specific force applied to a body and its angular rates)..
1314 *
1315 * @param timeInterval time interval between epochs expressed in seconds (s).
1316 * @param c body-to-ECI-frame coordinate transformation.
1317 * @param oldC previous body-to-ECI-frame coordinate transformation.
1318 * @param vx x coordinate of velocity of body frame with respect ECI frame, resolved
1319 * along ECI-frame axes.
1320 * @param vy y coordinate of velocity of body frame with respect ECI frame, resolved
1321 * along ECI-frame axes.
1322 * @param vz z coordinate of velocity of body frame with respect ECI frame, resolved
1323 * along ECI-frame axes.
1324 * @param oldVx x coordinate of previous velocity of body frame respect ECI frame,
1325 * resolved along ECI-frame axes.
1326 * @param oldVy y coordinate of previous velocity of body frame respect ECI frame,
1327 * resolved along ECI-frame axes.
1328 * @param oldVz z coordinate of previous velocity of body frame respect ECI frame,
1329 * resolved along ECI-frame axes.
1330 * @param position body position expressed in meters (m) with respect ECI frame,
1331 * resolved along ECI-frame axes.
1332 * @param result instance where estimated body kinematics will be stored.
1333 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
1334 * are not ECI frame valid.
1335 */
1336 public static void estimateKinematics(
1337 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
1338 final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
1339 final Point3D position, final BodyKinematics result) {
1340 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz,
1341 position.getInhomX(), position.getInhomY(), position.getInhomZ(), result);
1342 }
1343
1344 /**
1345 * Estimates body kinematics (specific force applied to a body and its angular rates)..
1346 *
1347 * @param timeInterval time interval between epochs.
1348 * @param c body-to-ECI-frame coordinate transformation.
1349 * @param oldC previous body-to-ECI-frame coordinate transformation.
1350 * @param vx x coordinate of velocity of body frame with respect ECI frame, resolved
1351 * along ECI-frame axes.
1352 * @param vy y coordinate of velocity of body frame with respect ECI frame, resolved
1353 * along ECI-frame axes.
1354 * @param vz z coordinate of velocity of body frame with respect ECI frame, resolved
1355 * along ECI-frame axes.
1356 * @param oldVx x coordinate of previous velocity of body frame respect ECI frame,
1357 * resolved along ECI-frame axes.
1358 * @param oldVy y coordinate of previous velocity of body frame respect ECI frame,
1359 * resolved along ECI-frame axes.
1360 * @param oldVz z coordinate of previous velocity of body frame respect ECI frame,
1361 * resolved along ECI-frame axes.
1362 * @param position body position expressed in meters (m) with respect ECI frame,
1363 * resolved along ECI-frame axes.
1364 * @param result instance where estimated body kinematics will be stored.
1365 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
1366 * are not ECI frame valid.
1367 */
1368 public static void estimateKinematics(
1369 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
1370 final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
1371 final Point3D position, final BodyKinematics result) {
1372 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz,
1373 position.getInhomX(), position.getInhomY(), position.getInhomZ(), result);
1374 }
1375
1376 /**
1377 * Estimates body kinematics (specific force applied to a body and its angular rates)..
1378 *
1379 * @param timeInterval time interval between epochs expressed in seconds (s).
1380 * @param c body-to-ECI-frame coordinate transformation.
1381 * @param oldC previous body-to-ECI-frame coordinate transformation.
1382 * @param vx x coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
1383 * along ECI-frame axes.
1384 * @param vy y coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
1385 * along ECI-frame axes.
1386 * @param vz z coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
1387 * along ECI-frame axes.
1388 * @param oldVx x coordinate of previous velocity of body frame expressed in meters per second (m/s) and
1389 * resolved along ECI-frame axes.
1390 * @param oldVy y coordinate of previous velocity of body frame expressed in meters per second (m/s) and
1391 * resolved along ECI-frame axes.
1392 * @param oldVz z coordinate of previous velocity of body frame expressed in meters per second (m/s) and
1393 * resolved along ECI-frame axes.
1394 * @param x cartesian x coordinate of body position expressed in meters (m) and resolved along
1395 * ECI-frame axes.
1396 * @param y cartesian y coordinate of body position expressed in meters (m) and resolved along
1397 * ECI-frame axes.
1398 * @param z cartesian z coordinate of body position expressed in meters (m) and resolved along
1399 * ECI-frame axes.
1400 * @return a new body kinematics instance.
1401 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
1402 * are not ECI frame valid.
1403 */
1404 public static BodyKinematics estimateKinematicsAndReturnNew(
1405 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
1406 final double vx, final double vy, final double vz,
1407 final double oldVx, final double oldVy, final double oldVz,
1408 final double x, final double y, final double z) {
1409 final var result = new BodyKinematics();
1410 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z, result);
1411 return result;
1412 }
1413
1414 /**
1415 * Estimates body kinematics (specific force applied to a body and its angular rates)..
1416 *
1417 * @param timeInterval time interval between epochs.
1418 * @param c body-to-ECI-frame coordinate transformation.
1419 * @param oldC previous body-to-ECI-frame coordinate transformation.
1420 * @param vx x coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
1421 * along ECI-frame axes.
1422 * @param vy y coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
1423 * along ECI-frame axes.
1424 * @param vz z coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
1425 * along ECI-frame axes.
1426 * @param oldVx x coordinate of previous velocity of body frame expressed in meters per second (m/s) and
1427 * resolved along ECI-frame axes.
1428 * @param oldVy y coordinate of previous velocity of body frame expressed in meters per second (m/s) and
1429 * resolved along ECI-frame axes.
1430 * @param oldVz z coordinate of previous velocity of body frame expressed in meters per second (m/s) and
1431 * resolved along ECI-frame axes.
1432 * @param x cartesian x coordinate of body position expressed in meters (m) and resolved along
1433 * ECI-frame axes.
1434 * @param y cartesian y coordinate of body position expressed in meters (m) and resolved along
1435 * ECI-frame axes.
1436 * @param z cartesian z coordinate of body position expressed in meters (m) and resolved along
1437 * ECI-frame axes.
1438 * @return a new body kinematics instance.
1439 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
1440 * are not ECI frame valid.
1441 */
1442 public static BodyKinematics estimateKinematicsAndReturnNew(
1443 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
1444 final double vx, final double vy, final double vz,
1445 final double oldVx, final double oldVy, final double oldVz,
1446 final double x, final double y, final double z) {
1447 final var result = new BodyKinematics();
1448 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z, result);
1449 return result;
1450 }
1451
1452 /**
1453 * Estimates body kinematics (specific force applied to a body and its angular rates).
1454 *
1455 * @param timeInterval time interval between epochs expressed in seconds (s).
1456 * @param frame body ECI frame containing current position, velocity and
1457 * body-to-ECI frame coordinate transformation.
1458 * @param oldC previous body-to-ECI-frame coordinate transformation.
1459 * @param oldVx x coordinate of previous velocity of body frame expressed in meters per second (m/s) and
1460 * resolved along ECI-frame axes.
1461 * @param oldVy y coordinate of previous velocity of body frame expressed in meters per second (m/s) and
1462 * resolved along ECI-frame axes.
1463 * @param oldVz z coordinate of previous velocity of body frame expressed in meters per second (m/s) and
1464 * resolved along ECI-frame axes.
1465 * @return a new body kinematics instance.
1466 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
1467 * are not ECI frame valid.
1468 */
1469 public static BodyKinematics estimateKinematicsAndReturnNew(
1470 final double timeInterval, final ECIFrame frame, final CoordinateTransformation oldC,
1471 final double oldVx, final double oldVy, final double oldVz) {
1472 final var result = new BodyKinematics();
1473 estimateKinematics(timeInterval, frame, oldC, oldVx, oldVy, oldVz, result);
1474 return result;
1475 }
1476
1477 /**
1478 * Estimates body kinematics (specific force applied to a body and its angular rates).
1479 *
1480 * @param timeInterval time interval between epochs.
1481 * @param frame body ECI frame containing current position, velocity and
1482 * body-to-ECI frame coordinate transformation.
1483 * @param oldC previous body-to-ECI-frame coordinate transformation.
1484 * @param oldVx x coordinate of previous velocity of body frame expressed in meters per second (m/s) and
1485 * resolved along ECI-frame axes.
1486 * @param oldVy y coordinate of previous velocity of body frame expressed in meters per second (m/s) and
1487 * resolved along ECI-frame axes.
1488 * @param oldVz z coordinate of previous velocity of body frame expressed in meters per second (m/s) and
1489 * resolved along ECI-frame axes.
1490 * @return a new body kinematics instance.
1491 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
1492 * are not ECI frame valid.
1493 */
1494 public static BodyKinematics estimateKinematicsAndReturnNew(
1495 final Time timeInterval, final ECIFrame frame, final CoordinateTransformation oldC,
1496 final double oldVx, final double oldVy, final double oldVz) {
1497 final var result = new BodyKinematics();
1498 estimateKinematics(timeInterval, frame, oldC, oldVx, oldVy, oldVz, result);
1499 return result;
1500 }
1501
1502 /**
1503 * Estimates body kinematics (specific force applied to a body and its angular rates).
1504 *
1505 * @param timeInterval time interval between epochs expressed in seconds (s).
1506 * @param frame body ECI frame containing current position, velocity and
1507 * body-to-ECI frame coordinate transformation.
1508 * @param oldFrame body ECI frame containing previous position, velocity and
1509 * body-to-ECI frame coordinate transformation. Notice that
1510 * previous position contained in this frame is ignored.
1511 * @return a new body kinematics instance.
1512 * @throws IllegalArgumentException if provided time interval is negative.
1513 */
1514 public static BodyKinematics estimateKinematicsAndReturnNew(
1515 final double timeInterval, final ECIFrame frame, final ECIFrame oldFrame) {
1516 final var result = new BodyKinematics();
1517 estimateKinematics(timeInterval, frame, oldFrame, result);
1518 return result;
1519 }
1520
1521 /**
1522 * Estimates body kinematics (specific force applied to a body and its angular rates).
1523 *
1524 * @param timeInterval time interval between epochs.
1525 * @param frame body ECI frame containing current position, velocity and
1526 * body-to-ECI frame coordinate transformation.
1527 * @param oldFrame body ECI frame containing previous position, velocity and
1528 * body-to-ECI frame coordinate transformation. Notice that
1529 * previous position contained in this frame is ignored.
1530 * @return a new body kinematics instance.
1531 * @throws IllegalArgumentException if provided time interval is negative.
1532 */
1533 public static BodyKinematics estimateKinematicsAndReturnNew(
1534 final Time timeInterval, final ECIFrame frame, final ECIFrame oldFrame) {
1535 final var result = new BodyKinematics();
1536 estimateKinematics(timeInterval, frame, oldFrame, result);
1537 return result;
1538 }
1539
1540 /**
1541 * Estimates body kinematics (specific force applied to a body and its angular rates)..
1542 *
1543 * @param timeInterval time interval between epochs expressed in seconds (s).
1544 * @param c body-to-ECI-frame coordinate transformation.
1545 * @param oldC previous body-to-ECI-frame coordinate transformation.
1546 * @param vx x coordinate of velocity of body frame with respect ECI frame, resolved
1547 * along ECI-frame axes.
1548 * @param vy y coordinate of velocity of body frame with respect ECI frame, resolved
1549 * along ECI-frame axes.
1550 * @param vz z coordinate of velocity of body frame with respect ECI frame, resolved
1551 * along ECI-frame axes.
1552 * @param oldVx x coordinate of previous velocity of body frame with respect ECI frame,
1553 * resolved along ECI-frame axes.
1554 * @param oldVy y coordinate of previous velocity of body frame with respect ECI frame,
1555 * resolved along ECI-frame axes.
1556 * @param oldVz z coordinate of previous velocity of body frame with respect ECI frame,
1557 * resolved along ECI-frame axes.
1558 * @param x cartesian x coordinate of body position expressed in meters (m) and resolved along
1559 * ECI-frame axes.
1560 * @param y cartesian y coordinate of body position expressed in meters (m) and resolved along
1561 * ECI-frame axes.
1562 * @param z cartesian z coordinate of body position expressed in meters (m) and resolved along
1563 * ECI-frame axes.
1564 * @return a new body kinematics instance.
1565 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
1566 * are not ECI frame valid.
1567 */
1568 public static BodyKinematics estimateKinematicsAndReturnNew(
1569 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
1570 final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
1571 final double x, final double y, final double z) {
1572 final var result = new BodyKinematics();
1573 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z, result);
1574 return result;
1575 }
1576
1577 /**
1578 * Estimates body kinematics (specific force applied to a body and its angular rates)..
1579 *
1580 * @param timeInterval time interval between epochs.
1581 * @param c body-to-ECI-frame coordinate transformation.
1582 * @param oldC previous body-to-ECI-frame coordinate transformation.
1583 * @param vx x coordinate of velocity of body frame with respect ECI frame, resolved
1584 * along ECI-frame axes.
1585 * @param vy y coordinate of velocity of body frame with respect ECI frame, resolved
1586 * along ECI-frame axes.
1587 * @param vz z coordinate of velocity of body frame with respect ECI frame, resolved
1588 * along ECI-frame axes.
1589 * @param oldVx x coordinate of previous velocity of body frame with respect ECI frame,
1590 * resolved along ECI-frame axes.
1591 * @param oldVy y coordinate of previous velocity of body frame with respect ECI frame,
1592 * resolved along ECI-frame axes.
1593 * @param oldVz z coordinate of previous velocity of body frame with respect ECI frame,
1594 * resolved along ECI-frame axes.
1595 * @param x cartesian x coordinate of body position expressed in meters (m) and resolved along
1596 * ECI-frame axes.
1597 * @param y cartesian y coordinate of body position expressed in meters (m) and resolved along
1598 * ECI-frame axes.
1599 * @param z cartesian z coordinate of body position expressed in meters (m) and resolved along
1600 * ECI-frame axes.
1601 * @return a new body kinematics instance.
1602 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
1603 * are not ECI frame valid.
1604 */
1605 public static BodyKinematics estimateKinematicsAndReturnNew(
1606 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
1607 final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
1608 final double x, final double y, final double z) {
1609 final var result = new BodyKinematics();
1610 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z, result);
1611 return result;
1612 }
1613
1614 /**
1615 * Estimates body kinematics (specific force applied to a body and its angular rates).
1616 *
1617 * @param timeInterval time interval between epochs expressed in seconds (s).
1618 * @param frame body ECI frame containing current position, velocity and
1619 * body-to-ECI frame coordinate transformation.
1620 * @param oldC previous body-to-ECI-frame coordinate transformation.
1621 * @param oldVx x coordinate of previous velocity of body frame with respect ECI frame,
1622 * resolved along ECI-frame axes.
1623 * @param oldVy y coordinate of previous velocity of body frame with respect ECI frame,
1624 * resolved along ECI-frame axes.
1625 * @param oldVz z coordinate of previous velocity of body frame with respect ECI frame,
1626 * resolved along ECI-frame axes.
1627 * @return a new body kinematics instance.
1628 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
1629 * are not ECI frame valid.
1630 */
1631 public static BodyKinematics estimateKinematicsAndReturnNew(
1632 final double timeInterval, final ECIFrame frame,
1633 final CoordinateTransformation oldC, final Speed oldVx, final Speed oldVy, final Speed oldVz) {
1634 final var result = new BodyKinematics();
1635 estimateKinematics(timeInterval, frame, oldC, oldVx, oldVy, oldVz, result);
1636 return result;
1637 }
1638
1639 /**
1640 * Estimates body kinematics (specific force applied to a body and its angular rates).
1641 *
1642 * @param timeInterval time interval between epochs.
1643 * @param frame body ECI frame containing current position, velocity and
1644 * body-to-ECI frame coordinate transformation.
1645 * @param oldC previous body-to-ECI-frame coordinate transformation.
1646 * @param oldVx x coordinate of previous velocity of body frame with respect ECI frame,
1647 * resolved along ECI-frame axes.
1648 * @param oldVy y coordinate of previous velocity of body frame with respect ECI frame,
1649 * resolved along ECI-frame axes.
1650 * @param oldVz z coordinate of previous velocity of body frame with respect ECI frame,
1651 * resolved along ECI-frame axes.
1652 * @return a new body kinematics instance.
1653 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
1654 * are not ECI frame valid.
1655 */
1656 public static BodyKinematics estimateKinematicsAndReturnNew(
1657 final Time timeInterval, final ECIFrame frame, final CoordinateTransformation oldC,
1658 final Speed oldVx, final Speed oldVy, final Speed oldVz) {
1659 final var result = new BodyKinematics();
1660 estimateKinematics(timeInterval, frame, oldC, oldVx, oldVy, oldVz, result);
1661 return result;
1662 }
1663
1664 /**
1665 * Estimates body kinematics (specific force applied to a body and its angular rates)..
1666 *
1667 * @param timeInterval time interval between epochs expressed in seconds (s).
1668 * @param c body-to-ECI-frame coordinate transformation.
1669 * @param oldC previous body-to-ECI-frame coordinate transformation.
1670 * @param vx x coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
1671 * along ECI-frame axes.
1672 * @param vy y coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
1673 * along ECI-frame axes.
1674 * @param vz z coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
1675 * along ECI-frame axes.
1676 * @param oldVx x coordinate of previous velocity of body frame expressed in meters per second (m/s) and
1677 * resolved along ECI-frame axes.
1678 * @param oldVy y coordinate of previous velocity of body frame expressed in meters per second (m/s) and
1679 * resolved along ECI-frame axes.
1680 * @param oldVz z coordinate of previous velocity of body frame expressed in meters per second (m/s) and
1681 * resolved along ECI-frame axes.
1682 * @param position body position expressed in meters (m) with respect ECI frame,
1683 * resolved along ECI-frame axes.
1684 * @return a new body kinematics instance.
1685 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
1686 * are not ECI frame valid.
1687 */
1688 public static BodyKinematics estimateKinematicsAndReturnNew(
1689 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
1690 final double vx, final double vy, final double vz,
1691 final double oldVx, final double oldVy, final double oldVz, final Point3D position) {
1692 final var result = new BodyKinematics();
1693 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position, result);
1694 return result;
1695 }
1696
1697 /**
1698 * Estimates body kinematics (specific force applied to a body and its angular rates)..
1699 *
1700 * @param timeInterval time interval between epochs.
1701 * @param c body-to-ECI-frame coordinate transformation.
1702 * @param oldC previous body-to-ECI-frame coordinate transformation.
1703 * @param vx x coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
1704 * along ECI-frame axes.
1705 * @param vy y coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
1706 * along ECI-frame axes.
1707 * @param vz z coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
1708 * along ECI-frame axes.
1709 * @param oldVx x coordinate of previous velocity of body frame expressed in meters per second (m/s) and
1710 * resolved along ECI-frame axes.
1711 * @param oldVy y coordinate of previous velocity of body frame expressed in meters per second (m/s) and
1712 * resolved along ECI-frame axes.
1713 * @param oldVz z coordinate of previous velocity of body frame expressed in meters per second (m/s) and
1714 * resolved along ECI-frame axes.
1715 * @param position body position expressed in meters (m) with respect ECI frame,
1716 * resolved along ECI-frame axes.
1717 * @return a new body kinematics instance.
1718 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
1719 * are not ECI frame valid.
1720 */
1721 public static BodyKinematics estimateKinematicsAndReturnNew(
1722 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
1723 final double vx, final double vy, final double vz,
1724 final double oldVx, final double oldVy, final double oldVz, final Point3D position) {
1725 final var result = new BodyKinematics();
1726 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position, result);
1727 return result;
1728 }
1729
1730 /**
1731 * Estimates body kinematics (specific force applied to a body and its angular rates)..
1732 *
1733 * @param timeInterval time interval between epochs expressed in seconds (s).
1734 * @param c body-to-ECI-frame coordinate transformation.
1735 * @param oldC previous body-to-ECI-frame coordinate transformation.
1736 * @param vx x coordinate of velocity of body frame with respect ECI frame, resolved
1737 * along ECI-frame axes.
1738 * @param vy y coordinate of velocity of body frame with respect ECI frame, resolved
1739 * along ECI-frame axes.
1740 * @param vz z coordinate of velocity of body frame with respect ECI frame, resolved
1741 * along ECI-frame axes.
1742 * @param oldVx x coordinate of previous velocity of body frame respect ECI frame,
1743 * resolved along ECI-frame axes.
1744 * @param oldVy y coordinate of previous velocity of body frame respect ECI frame,
1745 * resolved along ECI-frame axes.
1746 * @param oldVz z coordinate of previous velocity of body frame respect ECI frame,
1747 * resolved along ECI-frame axes.
1748 * @param position body position expressed in meters (m) with respect ECI frame,
1749 * resolved along ECI-frame axes.
1750 * @return a new body kinematics instance.
1751 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
1752 * are not ECI frame valid.
1753 */
1754 public static BodyKinematics estimateKinematicsAndReturnNew(
1755 final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
1756 final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
1757 final Point3D position) {
1758 final var result = new BodyKinematics();
1759 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position, result);
1760 return result;
1761 }
1762
1763 /**
1764 * Estimates body kinematics (specific force applied to a body and its angular rates)..
1765 *
1766 * @param timeInterval time interval between epochs.
1767 * @param c body-to-ECI-frame coordinate transformation.
1768 * @param oldC previous body-to-ECI-frame coordinate transformation.
1769 * @param vx x coordinate of velocity of body frame with respect ECI frame, resolved
1770 * along ECI-frame axes.
1771 * @param vy y coordinate of velocity of body frame with respect ECI frame, resolved
1772 * along ECI-frame axes.
1773 * @param vz z coordinate of velocity of body frame with respect ECI frame, resolved
1774 * along ECI-frame axes.
1775 * @param oldVx x coordinate of previous velocity of body frame respect ECI frame,
1776 * resolved along ECI-frame axes.
1777 * @param oldVy y coordinate of previous velocity of body frame respect ECI frame,
1778 * resolved along ECI-frame axes.
1779 * @param oldVz z coordinate of previous velocity of body frame respect ECI frame,
1780 * resolved along ECI-frame axes.
1781 * @param position body position expressed in meters (m) with respect ECI frame,
1782 * resolved along ECI-frame axes.
1783 * @return a new body kinematics instance.
1784 * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
1785 * are not ECI frame valid.
1786 */
1787 public static BodyKinematics estimateKinematicsAndReturnNew(
1788 final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
1789 final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
1790 final Point3D position) {
1791 final var result = new BodyKinematics();
1792 estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position, result);
1793 return result;
1794 }
1795 }