View Javadoc
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;
17  
18  import com.irurueta.algebra.Matrix;
19  import com.irurueta.algebra.WrongSizeException;
20  import com.irurueta.navigation.inertial.calibration.AccelerationTriad;
21  import com.irurueta.navigation.inertial.calibration.AngularSpeedTriad;
22  import com.irurueta.units.Acceleration;
23  import com.irurueta.units.AccelerationConverter;
24  import com.irurueta.units.AccelerationUnit;
25  import com.irurueta.units.AngularSpeed;
26  import com.irurueta.units.AngularSpeedConverter;
27  import com.irurueta.units.AngularSpeedUnit;
28  
29  import java.io.Serial;
30  import java.io.Serializable;
31  import java.util.Objects;
32  
33  /**
34   * Describes the motion of a body based on the specific forces (i.e. specific acceleration) and
35   * angular rates applied to it.
36   * Body frame axes are typically defined so that x is the forward axis, pointing in the usual direction
37   * of travel, z is the down axis, pointing in the usual direction of gravity, and y is the right axis,
38   * completing the orthogonal set.
39   */
40  @SuppressWarnings("DuplicatedCode")
41  public class BodyKinematics implements Serializable, Cloneable {
42  
43      /**
44       * Number of components of specific force or angular rate.
45       */
46      public static final int COMPONENTS = 3;
47  
48      /**
49       * Serialization version. This is used to ensure compatibility of deserialization of permanently stored serialized
50       * instances.
51       */
52      @Serial
53      private static final long serialVersionUID = 0L;
54  
55      /**
56       * Specific force of body frame with respect ECI, ECEF or NED frame resolved along body-frame x-axis, averaged
57       * over time interval and expressed in meters per squared second (m/s^2).
58       */
59      private double fx;
60  
61      /**
62       * Specific force of body frame with respect ECI, ECEF or NED frame resolved along body-frame y-axis, averaged
63       * over time interval and expressed in meters per squared second (m/s^2).
64       */
65      private double fy;
66  
67      /**
68       * Specific force of body frame with respect ECI, ECEF or NED frame resolved along body-frame z-axis, averaged
69       * over time interval and expressed in meters per squared second (m/s^2).
70       */
71      private double fz;
72  
73      /**
74       * Angular rate of body frame with respect ECI, ECEF or NED frame, resolved about body-frame x-axis, averaged
75       * over time interval and expressed in radians per second (rad/s).
76       */
77      private double angularRateX;
78  
79      /**
80       * Angular rate of body frame with respect ECI, ECEF or NED frame, resolved about body-frame y-axis, averaged
81       * over time interval and expressed in radians per second (rad/s).
82       */
83      private double angularRateY;
84  
85      /**
86       * Angular rate of body frame with respect ECI, ECEF or NED frame, resolved about body-frame z-axis, averaged
87       * over time interval and expressed in radians per second (rad/s).
88       */
89      private double angularRateZ;
90  
91      /**
92       * Constructor.
93       */
94      public BodyKinematics() {
95      }
96  
97      /**
98       * Constructor.
99       *
100      * @param fx Specific force of body frame with respect ECI, ECEF or NED frame resolved along body-frame x-axis,
101      *           averaged over time interval and expressed in meters per squared second (m/s^2).
102      * @param fy Specific force of body frame with respect ECI, ECEF or NED frame resolved along body-frame y-axis,
103      *           averaged over time interval and expressed in meters per squared second (m/s^2).
104      * @param fz Specific force of body frame with respect ECI, ECEF or NED frame resolved along body-frame z-axis,
105      *           averaged over time interval and expressed in meters per squared second (m/s^2).
106      */
107     public BodyKinematics(final double fx, final double fy, final double fz) {
108         setSpecificForceCoordinates(fx, fy, fz);
109     }
110 
111     /**
112      * Constructor.
113      *
114      * @param fx           Specific force of body frame with respect ECI, ECEF or NED frame resolved along
115      *                     body-frame x-axis, averaged over time interval and expressed in meters per
116      *                     squared second (m/s^2).
117      * @param fy           Specific force of body frame with respect ECI, ECEF or NED frame resolved along
118      *                     body-frame y-axis, averaged over time interval and expressed in meters per
119      *                     squared second (m/s^2).
120      * @param fz           Specific force of body frame with respect ECI, ECEF or NED frame resolved along
121      *                     body-frame z-axis, averaged over time interval and expressed in meters per
122      *                     squared second (m/s^2).
123      * @param angularRateX Angular rate of body frame with respect ECI, ECEF or NED frame, resolved about
124      *                     body-frame x-axis, averaged over time interval and expressed in radians per
125      *                     second (rad/s).
126      * @param angularRateY Angular rate of body frame with respect ECI, ECEF or NED frame, resolved about
127      *                     body-frame y-axis, averaged over time interval and expressed in radians per
128      *                     second (rad/s).
129      * @param angularRateZ Angular rate of body frame with respect ECI, ECEF or NED frame, resolved about
130      *                     body-frame z-axis, averaged over time interval and expressed in radians per
131      *                     second (rad/s).
132      */
133     public BodyKinematics(final double fx, final double fy, final double fz,
134                           final double angularRateX, final double angularRateY, final double angularRateZ) {
135         setSpecificForceCoordinates(fx, fy, fz);
136         setAngularRateCoordinates(angularRateX, angularRateY, angularRateZ);
137     }
138 
139     /**
140      * Constructor.
141      *
142      * @param specificForceX Specific force of body frame with respect ECI, ECEF or NED frame resolved
143      *                       along body-frame x-axis, averaged over time interval.
144      * @param specificForceY Specific force of body frame with respect ECI, ECEF or NED frame resolved
145      *                       along body-frame y-axis, averaged over time interval.
146      * @param specificForceZ Specific force of body frame with respect ECI, ECEF or NED frame resolved
147      *                       along body-frame z-axis, averaged over time interval.
148      */
149     public BodyKinematics(
150             final Acceleration specificForceX, final Acceleration specificForceY, final Acceleration specificForceZ) {
151         setSpecificForceCoordinates(specificForceX, specificForceY, specificForceZ);
152     }
153 
154     /**
155      * Constructor.
156      *
157      * @param angularSpeedX Angular speed of body frame with respect ECI, ECEF or NED frame, resolved
158      *                      about body-frame x-axis, averaged over time interval.
159      * @param angularSpeedY Angular speed of body frame with respect ECI, ECEF or NED frame, resolved
160      *                      about body-frame y-axis, averaged over time interval.
161      * @param angularSpeedZ Angular speed of body frame with respect ECI, ECEF or NED frame, resolved
162      *                      about body-frame z-axis, averaged over time interval.
163      */
164     public BodyKinematics(
165             final AngularSpeed angularSpeedX, final AngularSpeed angularSpeedY, final AngularSpeed angularSpeedZ) {
166         setAngularSpeedCoordinates(angularSpeedX, angularSpeedY, angularSpeedZ);
167     }
168 
169     /**
170      * Constructor.
171      *
172      * @param specificForceX Specific force of body frame with respect ECI, ECEF or NED frame resolved
173      *                       along body-frame x-axis, averaged over time interval.
174      * @param specificForceY Specific force of body frame with respect ECI, ECEF or NED frame resolved
175      *                       along body-frame y-axis, averaged over time interval.
176      * @param specificForceZ Specific force of body frame with respect ECI, ECEF or NED frame resolved
177      *                       along body-frame z-axis, averaged over time interval.
178      * @param angularSpeedX  Angular speed of body frame with respect ECI, ECEF or NED frame, resolved
179      *                       about body-frame x-axis, averaged over time interval.
180      * @param angularSpeedY  Angular speed of body frame with respect ECI, ECEF or NED frame, resolved
181      *                       about body-frame y-axis, averaged over time interval.
182      * @param angularSpeedZ  Angular speed of body frame with respect ECI, ECEF or NED frame, resolved
183      *                       about body-frame z-axis, averaged over time interval.
184      */
185     public BodyKinematics(
186             final Acceleration specificForceX, final Acceleration specificForceY, final Acceleration specificForceZ,
187             final AngularSpeed angularSpeedX, final AngularSpeed angularSpeedY, final AngularSpeed angularSpeedZ) {
188         setSpecificForceCoordinates(specificForceX, specificForceY, specificForceZ);
189         setAngularSpeedCoordinates(angularSpeedX, angularSpeedY, angularSpeedZ);
190     }
191 
192     /**
193      * Constructor.
194      *
195      * @param specificForceTriad specific force triad.
196      * @param angularSpeedTriad  angular speed triad.
197      */
198     public BodyKinematics(
199             final AccelerationTriad specificForceTriad, final AngularSpeedTriad angularSpeedTriad) {
200         setSpecificForceTriad(specificForceTriad);
201         setAngularRateTriad(angularSpeedTriad);
202     }
203 
204     /**
205      * Constructor.
206      *
207      * @param input instance to copy data from.
208      */
209     public BodyKinematics(final BodyKinematics input) {
210         copyFrom(input);
211     }
212 
213     /**
214      * Gets specific force of body frame with respect ECI, ECEF or NED frame resolved along body-frame x-axis, averaged
215      * over time interval and expressed in meters per squared second (m/s^2).
216      *
217      * @return specific force of body frame with respect ECI, ECEF or NED frame resolved along body-frame x-axis.
218      */
219     public double getFx() {
220         return fx;
221     }
222 
223     /**
224      * Sets specific force of body frame with respect ECI, ECEF or NED frame resolved along body-frame x-axis, averaged
225      * over time interval and expressed in meters per squared second (m/s^2).
226      *
227      * @param fx specific force of body frame with respect ECI, ECEF or NED frame resolved along body-frame x-axis.
228      */
229     public void setFx(final double fx) {
230         this.fx = fx;
231     }
232 
233     /**
234      * Gets specific force of body frame with respect ECI, ECEF or NED frame resolved along body-frame y-axis, averaged
235      * over time interval and expressed in meters per squared second (m/s^2).
236      *
237      * @return specific force of body frame with respect ECI, ECEF or NED frame resolved along body-frame y-axis.
238      */
239     public double getFy() {
240         return fy;
241     }
242 
243     /**
244      * Sets specific force of body frame with respect ECI, ECEF or NED frame resolved along body-frame y-axis, averaged
245      * over time interval and expressed in meters per squared second (m/s^2).
246      *
247      * @param fy specific force of body frame with respect ECI, ECEF or NED frame resolved along body-frame y-axis.
248      */
249     public void setFy(final double fy) {
250         this.fy = fy;
251     }
252 
253     /**
254      * Gets specific force of body frame with respect ECI, ECEF or NED frame resolved along body-frame z-axis, averaged
255      * over time interval and expressed in meters per squared second (m/s^2).
256      *
257      * @return specific force of body frame with respect ECI, ECEF or NED frame resolved along body-frame z-axis.
258      */
259     public double getFz() {
260         return fz;
261     }
262 
263     /**
264      * Sets specific force of body frame with respect ECI, ECEF or NED frame resolved along body-frame z-axis, averaged
265      * over time interval and expressed in meters per squared second (m/s^2).
266      *
267      * @param fz specific force of body frame with respect ECI, ECEF or NED frame resolved along body-frame z-axis.
268      */
269     public void setFz(final double fz) {
270         this.fz = fz;
271     }
272 
273     /**
274      * Sets specific force coordinates of body frame with respect ECI, ECEF or NED frame resolved along body-frame
275      * axes, averaged over time interval and expressed in meters per squared second (m/s^2).
276      *
277      * @param fx Specific force of body frame with respect ECI, ECEF or NED frame resolved along body-frame x-axis,
278      *           averaged over time interval and expressed in meters per squared second (m/s^2).
279      * @param fy Specific force of body frame with respect ECI, ECEF or NED frame resolved along body-frame y-axis,
280      *           averaged over time interval and expressed in meters per squared second (m/s^2).
281      * @param fz Specific force of body frame with respect ECI, ECEF or NED frame resolved along body-frame z-axis,
282      *           averaged over time interval and expressed in meters per squared second (m/s^2).
283      */
284     public void setSpecificForceCoordinates(final double fx, final double fy, final double fz) {
285         this.fx = fx;
286         this.fy = fy;
287         this.fz = fz;
288     }
289 
290     /**
291      * Gets specific force of body frame with respect ECI, ECEF or NED frame resolved along body-frame x-axis, averaged
292      * over time interval.
293      *
294      * @param result instance where specific force of body frame with respect ECI, ECEF or NED frame resolved along
295      *               body-frame x-axis will be stored.
296      */
297     public void getSpecificForceX(final Acceleration result) {
298         result.setValue(fx);
299         result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
300     }
301 
302     /**
303      * Gets specific force of body frame with respect ECI, ECEF or NED frame resolved along body-frame x-axis, averaged
304      * over time interval.
305      *
306      * @return a new instance with specific force of body frame with respect ECI, ECEF or NED frame resolved along
307      * body-frame x-axis.
308      */
309     public Acceleration getSpecificForceX() {
310         return new Acceleration(fx, AccelerationUnit.METERS_PER_SQUARED_SECOND);
311     }
312 
313     /**
314      * Sets specific force of body frame with respect ECI, ECEF or NED frame resolved along body-frame x-axis, averaged
315      * over time interval.
316      *
317      * @param specificForceX specific force of body frame with respect ECI, ECEF or NED frame resolved along
318      *                       body-frame x-axis that will be set.
319      */
320     public void setSpecificForceX(final Acceleration specificForceX) {
321         fx = AccelerationConverter.convert(specificForceX.getValue().doubleValue(), specificForceX.getUnit(),
322                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
323     }
324 
325     /**
326      * Gets specific force of body frame with respect ECI, ECEF or NED frame resolved along body-frame y-axis, averaged
327      * over time interval.
328      *
329      * @param result instance where specific force of body frame with respect ECI, ECEF or NED frame resolved along
330      *               body-frame y-axis will be stored.
331      */
332     public void getSpecificForceY(final Acceleration result) {
333         result.setValue(fy);
334         result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
335     }
336 
337     /**
338      * Gets specific force of body frame with respect ECI, ECEF or NED frame resolved along body-frame y-axis, averaged
339      * over time interval.
340      *
341      * @return a new instance with specific force of body frame with respect ECI, ECEF or NED frame resolved along
342      * body-frame y-axis.
343      */
344     public Acceleration getSpecificForceY() {
345         return new Acceleration(fy, AccelerationUnit.METERS_PER_SQUARED_SECOND);
346     }
347 
348     /**
349      * Sets specific force of body frame with respect ECI, ECEF or NED frame resolved along body frame y-axis, averaged
350      * over time interval.
351      *
352      * @param specificForceY specific force of body frame with respect ECI, ECEF or NED frame resolved along
353      *                       body-frame y-axis that will be set.
354      */
355     public void setSpecificForceY(final Acceleration specificForceY) {
356         fy = AccelerationConverter.convert(specificForceY.getValue().doubleValue(), specificForceY.getUnit(),
357                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
358     }
359 
360     /**
361      * Gets specific force of body frame with respect ECI, ECEF or NED frame resolved along body-frame z-axis, averaged
362      * over time interval.
363      *
364      * @param result instance where specific force of body frame with respect ECI, ECEF or NED frame resolved along
365      *               body-frame z-axis will be stored.
366      */
367     public void getSpecificForceZ(final Acceleration result) {
368         result.setValue(fz);
369         result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
370     }
371 
372     /**
373      * Gets specific force of body frame with respect ECI, ECEF or NED frame resolved along body-frame z-axis, averaged
374      * over time interval.
375      *
376      * @return a new instance with specific force of body frame with respect ECI, ECEF or NED frame resolved along
377      * body-frame z-axis.
378      */
379     public Acceleration getSpecificForceZ() {
380         return new Acceleration(fz, AccelerationUnit.METERS_PER_SQUARED_SECOND);
381     }
382 
383     /**
384      * Sets specific force of body frame with respect ECI, ECEF or NED frame resolved along body-frame z-axis, averaged
385      * over time interval.
386      *
387      * @param specificForceZ specific force of body frame with respect ECI, ECEF or NED frame resolved along
388      *                       body-frame z-axis that will be set.
389      */
390     public void setSpecificForceZ(final Acceleration specificForceZ) {
391         fz = AccelerationConverter.convert(specificForceZ.getValue().doubleValue(), specificForceZ.getUnit(),
392                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
393     }
394 
395     /**
396      * Sets specific force coordinates of body frame with respect ECI, ECEF or NED frame resolved along body-frame axes,
397      * averaged over time interval.
398      *
399      * @param specificForceX Specific force of body frame with respect ECI, ECEF or NED frame resolved along body-frame
400      *                       x-axis, averaged over time interval.
401      * @param specificForceY Specific force of body frame with respect ECI, ECEF or NED frame resolved along body-frame
402      *                       y-axis, averaged over time interval.
403      * @param specificForceZ Specific force of body frame with respect ECI, ECEF or NED frame resolved along body-frame
404      *                       z-axis, averaged over time interval.
405      */
406     public void setSpecificForceCoordinates(
407             final Acceleration specificForceX, final Acceleration specificForceY, final Acceleration specificForceZ) {
408         setSpecificForceX(specificForceX);
409         setSpecificForceY(specificForceY);
410         setSpecificForceZ(specificForceZ);
411     }
412 
413     /**
414      * Gets specific force triad of accelerometer measurements.
415      *
416      * @return specific force triad.
417      */
418     public AccelerationTriad getSpecificForceTriad() {
419         return new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND, fx, fy, fz);
420     }
421 
422     /**
423      * Gets specific force triad of accelerometer measurements.
424      *
425      * @param result instance where result will be stored.
426      */
427     public void getSpecificForceTriad(final AccelerationTriad result) {
428         result.setValueCoordinatesAndUnit(fx, fy, fz, AccelerationUnit.METERS_PER_SQUARED_SECOND);
429     }
430 
431     /**
432      * Sets specific force triad of accelerometer measurements.
433      *
434      * @param triad specific force triad.
435      */
436     public void setSpecificForceTriad(final AccelerationTriad triad) {
437         final AccelerationUnit unit = triad.getUnit();
438         fx = AccelerationConverter.convert(triad.getValueX(), unit, AccelerationUnit.METERS_PER_SQUARED_SECOND);
439         fy = AccelerationConverter.convert(triad.getValueY(), unit, AccelerationUnit.METERS_PER_SQUARED_SECOND);
440         fz = AccelerationConverter.convert(triad.getValueZ(), unit, AccelerationUnit.METERS_PER_SQUARED_SECOND);
441     }
442 
443     /**
444      * Gets angular rate of body frame with respect ECI, ECEF or NED frame, resolved about body-frame x-axis, averaged
445      * over time interval and expressed in radians per second (rad/s).
446      *
447      * @return angular rate of body frame with respect ECI, ECEF or NED frame resolved about body-frame x-axis.
448      */
449     public double getAngularRateX() {
450         return angularRateX;
451     }
452 
453     /**
454      * Sets angular rate of body frame with respect ECI, ECEF or NED frame, resolved about body-frame x-axis,
455      * over time interval and expressed in radians per second (rad/s).
456      *
457      * @param angularRateX angular rate of body frame with respect ECI, ECEF or NED frame resolved
458      *                     about body-frame x-axis.
459      */
460     public void setAngularRateX(final double angularRateX) {
461         this.angularRateX = angularRateX;
462     }
463 
464     /**
465      * Gets angular rate of body frame with respect ECI, ECEF or NED frame, resolved about body-frame y-axis, averaged
466      * over time interval and expressed in radians per second (rad/s).
467      *
468      * @return angular rate of body frame with respect ECI, ECEF or NED frame resolved about body-frame y-axis.
469      */
470     public double getAngularRateY() {
471         return angularRateY;
472     }
473 
474     /**
475      * Sets angular rate of body frame with respect ECI, ECEF or NED frame, resolved about body-frame y-axis, averaged
476      * over time interval and expressed in radians per second (rad/s).
477      *
478      * @param angularRateY angular rate of body frame with respect ECI, ECEF or NED frame resolved
479      *                     about body-frame y-axis.
480      */
481     public void setAngularRateY(final double angularRateY) {
482         this.angularRateY = angularRateY;
483     }
484 
485     /**
486      * Gets angular rate of body frame with respect ECI, ECEF or NED frame, resolved about body-frame z-axis, averaged
487      * over time interval and expressed in radians per second (rad/s).
488      *
489      * @return angular rate of body frame with respect ECI, ECEF or NED frame resolved about body-frame z-axis.
490      */
491     public double getAngularRateZ() {
492         return angularRateZ;
493     }
494 
495     /**
496      * Sets angular rate of body frame with respect ECI, ECEF or NED frame, resolved about body-frame z-axis, averaged
497      * over time interval and expressed in radians per second (rad/s).
498      *
499      * @param angularRateZ angular rate of body frame with respect ECI, ECEF or NED frame resolved
500      *                     about body-frame z-axis.
501      */
502     public void setAngularRateZ(final double angularRateZ) {
503         this.angularRateZ = angularRateZ;
504     }
505 
506     /**
507      * Sets angular rate coordinates of body frame with respect ECI, ECEF or NED frame, resolved about body-frame axes,
508      * averaged over time interval and expressed in radians per second (rad/s).
509      *
510      * @param angularRateX Angular rate of body frame with respect ECI, ECEF or NED frame, resolved about body-frame
511      *                     x-axis, averaged over time interval and expressed in radians per second (rad/s).
512      * @param angularRateY Angular rate of body frame with respect ECI, ECEF or NED frame, resolved about body-frame
513      *                     y-axis, averaged over time interval and expressed in radians per second (rad/s).
514      * @param angularRateZ Angular rate of body frame with respect ECI, ECEF or NED frame, resolved about body-frame
515      *                     z-axis, averaged over time interval and expressed in radians per second (rad/s).
516      */
517     public void setAngularRateCoordinates(
518             final double angularRateX, final double angularRateY, final double angularRateZ) {
519         this.angularRateX = angularRateX;
520         this.angularRateY = angularRateY;
521         this.angularRateZ = angularRateZ;
522     }
523 
524     /**
525      * Gets angular speed of body frame with respect ECI, ECEF or NED frame, resolved about body-frame x-axis, averaged
526      * over time interval.
527      *
528      * @param result instance where angular speed of body frame with respect ECI, ECEF or NED frame resolved about
529      *               body-frame x-axis will be stored.
530      */
531     public void getAngularSpeedX(final AngularSpeed result) {
532         result.setValue(angularRateX);
533         result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
534     }
535 
536     /**
537      * Gets angular speed of body frame with respect ECI, ECEF or NED frame, resolved about body-frame x-axis, averaged
538      * over time interval.
539      *
540      * @return a new instance of angular speed of body frame with respect ECI, ECEF or NED frame resolved about
541      * body-frame X-axis.
542      */
543     public AngularSpeed getAngularSpeedX() {
544         return new AngularSpeed(angularRateX, AngularSpeedUnit.RADIANS_PER_SECOND);
545     }
546 
547     /**
548      * Sets angular speed of body frame with respect ECI, ECEF or NED frame, resolved about body-frame x-axis, averaged
549      * over time interval.
550      *
551      * @param angularSpeedX angular speed of body frame with respect ECI, ECEF or NED frame resolved about body-frame
552      *                      x-axis that will be set.
553      */
554     public void setAngularSpeedX(final AngularSpeed angularSpeedX) {
555         angularRateX = AngularSpeedConverter.convert(angularSpeedX.getValue().doubleValue(), angularSpeedX.getUnit(),
556                 AngularSpeedUnit.RADIANS_PER_SECOND);
557     }
558 
559     /**
560      * Gets angular speed of body frame with respect ECI, ECEF or NED frame, resolved about body-frame y-axis, averaged
561      * over time interval.
562      *
563      * @param result instance where angular speed of body frame with respect ECI, ECEF or NED frame resolved about
564      *               body-frame y-axis will be stored.
565      */
566     public void getAngularSpeedY(final AngularSpeed result) {
567         result.setValue(angularRateY);
568         result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
569     }
570 
571     /**
572      * Gets angular speed of body frame with respect ECI, ECEF or NED frame, resolved about body-frame y-axis, averaged
573      * over time interval.
574      *
575      * @return a new instance of angular speed of body frame with respect ECI, ECEF or NED frame resolved about
576      * body-frame y-axis.
577      */
578     public AngularSpeed getAngularSpeedY() {
579         return new AngularSpeed(angularRateY, AngularSpeedUnit.RADIANS_PER_SECOND);
580     }
581 
582     /**
583      * Sets angular speed of body frame with respect ECI, ECEF or NED frame, resolved about body-frame y-axis, averaged
584      * over time interval.
585      *
586      * @param angularSpeedY angular speed of body frame with respect ECI, ECEF or NED frame resolved about body-frame
587      *                      y-axis that will be set.
588      */
589     public void setAngularSpeedY(final AngularSpeed angularSpeedY) {
590         angularRateY = AngularSpeedConverter.convert(angularSpeedY.getValue().doubleValue(), angularSpeedY.getUnit(),
591                 AngularSpeedUnit.RADIANS_PER_SECOND);
592     }
593 
594     /**
595      * Gets angular speed of body frame with respect ECI, ECEF or NED frame, resolved about body-frame z-axis, averaged
596      * over time interval.
597      *
598      * @param result instance where angular speed of body frame with respect ECI, ECEF or NED frame resolved about
599      *               body-frame z-axis will be stored.
600      */
601     public void getAngularSpeedZ(final AngularSpeed result) {
602         result.setValue(angularRateZ);
603         result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
604     }
605 
606     /**
607      * Gets angular speed of body frame with respect ECI, ECEF or NED frame, resolved about body-frame z-axis, averaged
608      * over time interval.
609      *
610      * @return a new instance of angular speed of body frame with respect ECI, ECEF or NED frame resolved about
611      * body-frame z-axis.
612      */
613     public AngularSpeed getAngularSpeedZ() {
614         return new AngularSpeed(angularRateZ, AngularSpeedUnit.RADIANS_PER_SECOND);
615     }
616 
617     /**
618      * Sets angular speed of body frame with respect ECI, ECEF or NED frame, resolved about body-frame y-axis, averaged
619      * over time interval.
620      *
621      * @param angularSpeedZ angular speed of body frame with respect ECI, ECEF or NED frame resolved about body-frame
622      *                      z-axis, that will be set.
623      */
624     public void setAngularSpeedZ(final AngularSpeed angularSpeedZ) {
625         angularRateZ = AngularSpeedConverter.convert(angularSpeedZ.getValue().doubleValue(), angularSpeedZ.getUnit(),
626                 AngularSpeedUnit.RADIANS_PER_SECOND);
627     }
628 
629     /**
630      * Sets angular speed coordinates of body frame with respect ECI, ECEF or NED frame, resolved about body-frame axes,
631      * averaged over time interval.
632      *
633      * @param angularSpeedX Angular speed of body frame with respect ECI, ECEF or NED frame, resolved about body-frame
634      *                      x-axis, averaged over time interval.
635      * @param angularSpeedY Angular speed of body frame with respect ECI, ECEF or NED frame, resolved about body-frame
636      *                      y-axis, averaged over time interval.
637      * @param angularSpeedZ Angular speed of body frame with respect ECI, ECEF or NED frame, resolved about body-frame
638      *                      z-axis, averaged over time interval.
639      */
640     public void setAngularSpeedCoordinates(
641             final AngularSpeed angularSpeedX, final AngularSpeed angularSpeedY, final AngularSpeed angularSpeedZ) {
642         setAngularSpeedX(angularSpeedX);
643         setAngularSpeedY(angularSpeedY);
644         setAngularSpeedZ(angularSpeedZ);
645     }
646 
647     /**
648      * Gets angular rate triad of gyroscope measurements.
649      *
650      * @return angular rate triad.
651      */
652     public AngularSpeedTriad getAngularRateTriad() {
653         return new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND, angularRateX, angularRateY, angularRateZ);
654     }
655 
656     /**
657      * Gets angular rate triad of gyroscope measurements.
658      *
659      * @param result angular rate triad.
660      */
661     public void getAngularRateTriad(final AngularSpeedTriad result) {
662         result.setValueCoordinatesAndUnit(angularRateX, angularRateY, angularRateZ,
663                 AngularSpeedUnit.RADIANS_PER_SECOND);
664     }
665 
666     /**
667      * Sets angular rate triad of gyroscope measurements.
668      *
669      * @param triad angular rate triad.
670      */
671     public void setAngularRateTriad(final AngularSpeedTriad triad) {
672         final AngularSpeedUnit unit = triad.getUnit();
673         angularRateX = AngularSpeedConverter.convert(triad.getValueX(), unit, AngularSpeedUnit.RADIANS_PER_SECOND);
674         angularRateY = AngularSpeedConverter.convert(triad.getValueY(), unit, AngularSpeedUnit.RADIANS_PER_SECOND);
675         angularRateZ = AngularSpeedConverter.convert(triad.getValueZ(), unit, AngularSpeedUnit.RADIANS_PER_SECOND);
676     }
677 
678     /**
679      * Gets norm of specific force expressed in meters per squared second (m/s^2).
680      *
681      * @return norm of specific force.
682      */
683     public double getSpecificForceNorm() {
684         return Math.sqrt(fx * fx + fy * fy + fz * fz);
685     }
686 
687     /**
688      * Gets norm of specific force.
689      *
690      * @param result instance where result will be stored.
691      */
692     public void getSpecificForceNormAsAcceleration(final Acceleration result) {
693         result.setValue(getSpecificForceNorm());
694         result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
695     }
696 
697     /**
698      * Gets norm of specific force.
699      *
700      * @return a new acceleration instance containing norm of specific force.
701      */
702     public Acceleration getSpecificForceNormAsAcceleration() {
703         return new Acceleration(getSpecificForceNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
704     }
705 
706     /**
707      * Gets norm of angular rate expressed in radians per second (rad/s).
708      *
709      * @return norm of angular rate.
710      */
711     public double getAngularRateNorm() {
712         return Math.sqrt(angularRateX * angularRateX + angularRateY * angularRateY + angularRateZ * angularRateZ);
713     }
714 
715     /**
716      * Gets norm of angular rate.
717      *
718      * @param result instance where norm of angular rate will be stored.
719      */
720     public void getAngularSpeedNorm(final AngularSpeed result) {
721         result.setValue(getAngularRateNorm());
722         result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
723     }
724 
725     /**
726      * Gets norm of angular rate.
727      *
728      * @return norm of angular rate.
729      */
730     public AngularSpeed getAngularSpeedNorm() {
731         return new AngularSpeed(getAngularRateNorm(), AngularSpeedUnit.RADIANS_PER_SECOND);
732     }
733 
734     /**
735      * Copies this instance data into provided instance.
736      *
737      * @param output destination instance where data will be copied to.
738      */
739     public void copyTo(final BodyKinematics output) {
740         output.fx = fx;
741         output.fy = fy;
742         output.fz = fz;
743         output.angularRateX = angularRateX;
744         output.angularRateY = angularRateY;
745         output.angularRateZ = angularRateZ;
746     }
747 
748     /**
749      * Copies data of provided instance into this instance.
750      *
751      * @param input instance to copy data from.
752      */
753     public void copyFrom(final BodyKinematics input) {
754         fx = input.fx;
755         fy = input.fy;
756         fz = input.fz;
757         angularRateX = input.angularRateX;
758         angularRateY = input.angularRateY;
759         angularRateZ = input.angularRateZ;
760     }
761 
762     /**
763      * Gets specific force coordinates expressed in meters per squared second (m/s^2) as an array.
764      *
765      * @param result array instance where specific force coordinates will be stored in
766      *               x, y, z order.
767      * @throws IllegalArgumentException if provided array does not have length 3.
768      */
769     public void asSpecificForceArray(final double[] result) {
770         if (result.length != COMPONENTS) {
771             throw new IllegalArgumentException();
772         }
773 
774         result[0] = fx;
775         result[1] = fy;
776         result[2] = fz;
777     }
778 
779     /**
780      * Gets specific force coordinates expressed in meters per squared second (m/s^2) as an array.
781      *
782      * @return array containing specific force coordinates in x,y,z order.
783      */
784     public double[] asSpecificForceArray() {
785         final var result = new double[COMPONENTS];
786         asSpecificForceArray(result);
787         return result;
788     }
789 
790     /**
791      * Gets specific force coordinates expressed in meters per squared second (m/s^2) as a column matrix.
792      * If provided matrix does not have size 3x1, it will be resized.
793      *
794      * @param result matrix instance where gravity coordinates will be stored in
795      *               x,y,z order.
796      */
797     public void asSpecificForceMatrix(final Matrix result) {
798         if (result.getRows() != COMPONENTS || result.getColumns() != 1) {
799             try {
800                 result.resize(COMPONENTS, 1);
801             } catch (final WrongSizeException ignore) {
802                 // never happens
803             }
804         }
805 
806         result.setElementAtIndex(0, fx);
807         result.setElementAtIndex(1, fy);
808         result.setElementAtIndex(2, fz);
809     }
810 
811     /**
812      * Gets specific force coordinates expressed in meters per squared second (m/s^2) as a column matrix.
813      *
814      * @return a matrix containing specific force coordinates stored in x,y,z order.
815      */
816     public Matrix asSpecificForceMatrix() {
817         Matrix result;
818         try {
819             result = new Matrix(COMPONENTS, 1);
820             asSpecificForceMatrix(result);
821         } catch (final WrongSizeException ignore) {
822             // never happens
823             result = null;
824         }
825         return result;
826     }
827 
828     /**
829      * Gets angular rate coordinates expressed in radians per second (rad/s) as an array.
830      *
831      * @param result array instance where angular rate coordinates will be stored in
832      *               x,y,z order.
833      * @throws IllegalArgumentException if provided array does not have length 3.
834      */
835     public void asAngularRateArray(final double[] result) {
836         if (result.length != COMPONENTS) {
837             throw new IllegalArgumentException();
838         }
839 
840         result[0] = angularRateX;
841         result[1] = angularRateY;
842         result[2] = angularRateZ;
843     }
844 
845     /**
846      * Gets angular rate coordinates expressed in radians per second (rad/s) as an array.
847      *
848      * @return array containing angular rate coordinates in x,y,z order.
849      */
850     public double[] asAngularRateArray() {
851         final var result = new double[COMPONENTS];
852         asAngularRateArray(result);
853         return result;
854     }
855 
856     /**
857      * Gets angular rate coordinates expressed in radians per second (rad/s) as a column matrix.
858      * If provided matrix does not have size 3x1, it will be resized.
859      *
860      * @param result matrix instance where angular rate coordinates will be stored in
861      *               x,y,z order.
862      */
863     public void asAngularRateMatrix(final Matrix result) {
864         if (result.getRows() != COMPONENTS || result.getColumns() != 1) {
865             try {
866                 result.resize(COMPONENTS, 1);
867             } catch (final WrongSizeException ignore) {
868                 // never happens
869             }
870         }
871 
872         result.setElementAtIndex(0, angularRateX);
873         result.setElementAtIndex(1, angularRateY);
874         result.setElementAtIndex(2, angularRateZ);
875     }
876 
877     /**
878      * Gets angular rate coordinates expressed in radians per second (rad/s) as a column matrix.
879      *
880      * @return a matrix containing angular rate coordinates stored in x,y,z order.
881      */
882     public Matrix asAngularRateMatrix() {
883         Matrix result;
884         try {
885             result = new Matrix(COMPONENTS, 1);
886             asAngularRateMatrix(result);
887         } catch (final WrongSizeException ignore) {
888             // never happens
889             result = null;
890         }
891         return result;
892     }
893 
894     /**
895      * Computes and returns hash code for this instance. Hash codes are almost unique
896      * values that are useful for fast classification and storage of objects in collections.
897      *
898      * @return Hash code.
899      */
900     @Override
901     public int hashCode() {
902         return Objects.hash(fx, fy, fz, angularRateX, angularRateY, angularRateZ);
903     }
904 
905     /**
906      * Checks if provided instance has exactly the same contents as this instance.
907      *
908      * @param other instance to be compared.
909      * @return true if both instances are considered to be equal, false otherwise.
910      */
911     public boolean equals(final BodyKinematics other) {
912         return equals(other, 0.0);
913     }
914 
915     /**
916      * Checks if provided instance has contents similar to this instance up to provided
917      * threshold value.
918      *
919      * @param other     instance to be compared.
920      * @param threshold maximum allowed difference between specific force and angular
921      *                  rate coordinates.
922      * @return true if both instances are considered to be equal (up to provided
923      * threshold), false otherwise.
924      */
925     public boolean equals(final BodyKinematics other, final double threshold) {
926         if (other == null) {
927             return false;
928         }
929 
930         return Math.abs(fx - other.fx) <= threshold && Math.abs(fy - other.fy) <= threshold
931                 && Math.abs(fz - other.fz) <= threshold && Math.abs(angularRateX - other.angularRateX) <= threshold
932                 && Math.abs(angularRateY - other.angularRateY) <= threshold
933                 && Math.abs(angularRateZ - other.angularRateZ) <= threshold;
934     }
935 
936     /**
937      * Checks if provided object is a BodyKinematics instance having exactly the same contents
938      * as this instance.
939      *
940      * @param obj object to be compared.
941      * @return true if both objects are considered to be equal, false otherwise.
942      */
943     @Override
944     public boolean equals(final Object obj) {
945         if (this == obj) {
946             return true;
947         }
948         if (obj == null || getClass() != obj.getClass()) {
949             return false;
950         }
951 
952         final BodyKinematics other = (BodyKinematics) obj;
953         return equals(other);
954     }
955 
956     /**
957      * Makes a copy of this instance.
958      *
959      * @return a copy of this instance.
960      * @throws CloneNotSupportedException if clone fails for some reason.
961      */
962     @Override
963     protected Object clone() throws CloneNotSupportedException {
964         final var result = (BodyKinematics) super.clone();
965         copyTo(result);
966         return result;
967     }
968 }