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.gnss;
17
18 import com.irurueta.algebra.Matrix;
19 import com.irurueta.algebra.WrongSizeException;
20 import com.irurueta.geometry.InhomogeneousPoint3D;
21 import com.irurueta.geometry.Point3D;
22 import com.irurueta.navigation.frames.ECEFPosition;
23 import com.irurueta.navigation.frames.ECEFVelocity;
24 import com.irurueta.units.Distance;
25 import com.irurueta.units.DistanceConverter;
26 import com.irurueta.units.DistanceUnit;
27 import com.irurueta.units.Speed;
28 import com.irurueta.units.SpeedConverter;
29 import com.irurueta.units.SpeedUnit;
30
31 import java.io.Serializable;
32 import java.util.Objects;
33
34 /**
35 * Contains GNSS state estimation, which contains user
36 * position, velocity and estimated clock offset and drift.
37 */
38 public class GNSSEstimation implements Serializable, Cloneable {
39
40 /**
41 * Number of parameters stored into Kalman filter state.
42 */
43 public static final int NUM_PARAMETERS = 8;
44
45 /**
46 * X coordinate of estimated ECEF user position expressed in meters (m).
47 */
48 private double x;
49
50 /**
51 * Y coordinate of estimated ECEF user position expressed in meters (m).
52 */
53 private double y;
54
55 /**
56 * Z coordinate of estimated ECEF user position expressed in meters (m).
57 */
58 private double z;
59
60 /**
61 * X coordinate of estimated ECEF user velocity expressed in meters per second (m/s).
62 */
63 private double vx;
64
65 /**
66 * Y coordinate of estimated ECEF user velocity expressed in meters per second (m/s).
67 */
68 private double vy;
69
70 /**
71 * Z coordinate of estimated ECEF user velocity expressed in meters per second (m/s).
72 */
73 private double vz;
74
75 /**
76 * Estimated receiver clock offset expressed in meters (m).
77 */
78 private double clockOffset;
79
80 /**
81 * Estimated receiver clock drift expressed in meters per second (m/s).
82 */
83 private double clockDrift;
84
85 /**
86 * Constructor.
87 */
88 public GNSSEstimation() {
89 }
90
91 /**
92 * Constructor.
93 *
94 * @param x x coordinate of estimated ECEF user position expressed in meters (m).
95 * @param y y coordinate of estimated ECEF user position expressed in meters (m).
96 * @param z z coordinate of estimated ECEF user position expressed in meters (m).
97 * @param vx x coordinate of estimated ECEF user velocity expressed in meters per second (m/s).
98 * @param vy y coordinate of estimated ECEF user velocity expressed in meters per second (m/s).
99 * @param vz z coordinate of estimated ECEF user velocity expressed in meters per second (m/s).
100 * @param clockOffset estimated receiver clock offset expressed in meters (m).
101 * @param clockDrift estimated receiver clock drift expressed in meters per second (m/s).
102 */
103 public GNSSEstimation(final double x, final double y, final double z,
104 final double vx, final double vy, final double vz,
105 final double clockOffset, final double clockDrift) {
106 setPositionCoordinates(x, y, z);
107 setVelocityCoordinates(vx, vy, vz);
108 setClockOffset(clockOffset);
109 setClockDrift(clockDrift);
110 }
111
112 /**
113 * Constructor.
114 *
115 * @param x x coordinate of estimated ECEF user position.
116 * @param y y coordinate of estimated ECEF user position.
117 * @param z z coordinate of estimated ECEF user position.
118 * @param vx x coordinate of estimated ECEF user velocity.
119 * @param vy y coordinate of estimated ECEF user velocity.
120 * @param vz z coordinate of estimated ECEF user velocity.
121 * @param clockOffset estimated receiver clock offset.
122 * @param clockDrift estimated receiver clock drift.
123 */
124 public GNSSEstimation(final Distance x, final Distance y, final Distance z,
125 final Speed vx, final Speed vy, final Speed vz,
126 final Distance clockOffset, final Speed clockDrift) {
127 setPositionCoordinates(x, y, z);
128 setVelocityCoordinates(vx, vy, vz);
129 setClockOffset(clockOffset);
130 setClockDrift(clockDrift);
131 }
132
133 /**
134 * Constructor.
135 *
136 * @param position estimated ECEF user position.
137 * @param vx x coordinate of estimated ECEF user velocity
138 * expressed in meters per second (m/s).
139 * @param vy y coordinate of estimated ECEF user velocity
140 * expressed in meters per second (m/s).
141 * @param vz z coordinate of estimated ECEF user velocity
142 * expressed in meters per second (m/s).
143 * @param clockOffset estimated receiver clock offset expressed in
144 * meters (m).
145 * @param clockDrift estimated receiver clock drift expressed in
146 * meters per second (m/s).
147 */
148 public GNSSEstimation(final Point3D position, final double vx, final double vy, final double vz,
149 final double clockOffset, final double clockDrift) {
150 setPosition(position);
151 setVelocityCoordinates(vx, vy, vz);
152 setClockOffset(clockOffset);
153 setClockDrift(clockDrift);
154 }
155
156 /**
157 * Constructor.
158 *
159 * @param position estimated ECEF user position.
160 * @param vx x coordinate of estimated ECEF user velocity.
161 * @param vy y coordinate of estimated ECEF user velocity.
162 * @param vz z coordinate of estimated ECEF user velocity.
163 * @param clockOffset estimated receiver clock offset.
164 * @param clockDrift estimated receiver clock drift.
165 * 8
166 */
167 public GNSSEstimation(final Point3D position, final Speed vx, final Speed vy, final Speed vz,
168 final Distance clockOffset, final Speed clockDrift) {
169 setPosition(position);
170 setVelocityCoordinates(vx, vy, vz);
171 setClockOffset(clockOffset);
172 setClockDrift(clockDrift);
173 }
174
175 /**
176 * Constructor.
177 *
178 * @param position estimated ECEF user position.
179 * @param velocity estimated ECEF user velocity.
180 * @param clockOffset estimated receiver clock offset expressed in meters (m).
181 * @param clockDrift estimated receiver clock drift expressed in meters per
182 * second (m/s).
183 */
184 public GNSSEstimation(final ECEFPosition position, final ECEFVelocity velocity,
185 final double clockOffset, final double clockDrift) {
186 setEcefPosition(position);
187 setEcefVelocity(velocity);
188 setClockOffset(clockOffset);
189 setClockDrift(clockDrift);
190 }
191
192 /**
193 * Constructor.
194 *
195 * @param position estimated ECEF user position.
196 * @param velocity estimated ECEF user velocity.
197 * @param clockOffset estimated receiver clock offset.
198 * @param clockDrift estimated receiver clock drift.
199 */
200 public GNSSEstimation(final ECEFPosition position, final ECEFVelocity velocity, final Distance clockOffset,
201 final Speed clockDrift) {
202 setEcefPosition(position);
203 setEcefVelocity(velocity);
204 setClockOffset(clockOffset);
205 setClockDrift(clockDrift);
206 }
207
208 /**
209 * Constructor.
210 *
211 * @param positionAndVelocity estimated ECEF user position and velocity.
212 * @param clockOffset estimated receiver clock offset expressed in
213 * meters (m).
214 * @param clockDrift estimated receiver clock drift expressed in
215 * meters per second (m/s).
216 */
217 public GNSSEstimation(final ECEFPositionAndVelocity positionAndVelocity, final double clockOffset,
218 final double clockDrift) {
219 setPositionAndVelocity(positionAndVelocity);
220 setClockOffset(clockOffset);
221 setClockDrift(clockDrift);
222 }
223
224 /**
225 * Constructor.
226 *
227 * @param positionAndVelocity estimated ECEF user position and velocity.
228 * @param clockOffset estimated receiver clock offset.
229 * @param clockDrift estimated receiver clock drift.
230 */
231 public GNSSEstimation(final ECEFPositionAndVelocity positionAndVelocity, final Distance clockOffset,
232 final Speed clockDrift) {
233 setPositionAndVelocity(positionAndVelocity);
234 setClockOffset(clockOffset);
235 setClockDrift(clockDrift);
236 }
237
238 /**
239 * Copy constructor.
240 *
241 * @param input input instance to copy data from.
242 */
243 public GNSSEstimation(final GNSSEstimation input) {
244 copyFrom(input);
245 }
246
247 /**
248 * Gets x coordinate of estimated ECEF user position expressed in meters (m).
249 *
250 * @return x coordinate of estimated ECEF user position.
251 */
252 public double getX() {
253 return x;
254 }
255
256 /**
257 * Sets x coordinate of estimated ECEF user position expressed in meters (m).
258 *
259 * @param x x coordinate of estimated ECEF user position.
260 */
261 public void setX(final double x) {
262 this.x = x;
263 }
264
265 /**
266 * Gets y coordinate of estimated ECEF user position expressed in meters (m).
267 *
268 * @return y coordinate of estimated ECEF user position.
269 */
270 public double getY() {
271 return y;
272 }
273
274 /**
275 * Sets y coordinate of estimated ECEF user position expressed in meters (m).
276 *
277 * @param y y coordinate of estimated ECEF user position.
278 */
279 public void setY(final double y) {
280 this.y = y;
281 }
282
283 /**
284 * Gets z coordinate of estimated ECEF user position expressed in meters (m).
285 *
286 * @return z coordinate of estimated ECEF user position.
287 */
288 public double getZ() {
289 return z;
290 }
291
292 /**
293 * Sets z coordinate of estimated ECEF user position expressed in meters (m).
294 *
295 * @param z z coordinate of estimated ECEF user position.
296 */
297 public void setZ(final double z) {
298 this.z = z;
299 }
300
301 /**
302 * Sets coordinates of estimated ECEF user position expressed in meters (m).
303 *
304 * @param x x coordinate.
305 * @param y y coordinate.
306 * @param z z coordinate.
307 */
308 public void setPositionCoordinates(final double x, final double y, final double z) {
309 this.x = x;
310 this.y = y;
311 this.z = z;
312 }
313
314 /**
315 * Gets x coordinate of estimated ECEF user velocity expressed in meters per second (m/s).
316 *
317 * @return x coordinate of estimated ECEF user velocity.
318 */
319 public double getVx() {
320 return vx;
321 }
322
323 /**
324 * Sets x coordinate of estimated ECEF user velocity expressed in meters per second (m/s).
325 *
326 * @param vx x coordinate of estimated ECEF user velocity.
327 */
328 public void setVx(final double vx) {
329 this.vx = vx;
330 }
331
332 /**
333 * Gets y coordinate of estimated ECEF user velocity expressed in meters per second (m/s).
334 *
335 * @return y coordinate of estimated ECEF user velocity.
336 */
337 public double getVy() {
338 return vy;
339 }
340
341 /**
342 * Sets y coordinate of estimated ECEF user velocity expressed in meters per second (m/s).
343 *
344 * @param vy y coordinate of estimated ECEF user velocity.
345 */
346 public void setVy(final double vy) {
347 this.vy = vy;
348 }
349
350 /**
351 * Gets z coordinate of estimated ECEF user velocity expressed in meters per second (m/s).
352 *
353 * @return z coordinate of estimated ECEF user velocity.
354 */
355 public double getVz() {
356 return vz;
357 }
358
359 /**
360 * Sets z coordinate of estimated ECEF user velocity expressed in meters per second (m/s).
361 *
362 * @param vz z coordinate of estimated ECEF user velocity.
363 */
364 public void setVz(final double vz) {
365 this.vz = vz;
366 }
367
368 /**
369 * Sets coordinates of estimated ECEF user velocity expressed in meters per second (m/s).
370 *
371 * @param vx x coordinate.
372 * @param vy y coordinate.
373 * @param vz z coordinate.
374 */
375 public void setVelocityCoordinates(final double vx, final double vy, final double vz) {
376 this.vx = vx;
377 this.vy = vy;
378 this.vz = vz;
379 }
380
381 /**
382 * Gets estimated receiver clock offset expressed in meters (m).
383 * Notice that clock offset is estimated in terms of distance, since timing errors
384 * will ultimately be propagated as distance errors.
385 *
386 * @return estimated receiver clock offset.
387 */
388 public double getClockOffset() {
389 return clockOffset;
390 }
391
392 /**
393 * Sets estimated receiver clock offset expressed in meters (m).
394 * Notice that clock offset is estimated in terms of distance, since timing errors
395 * are ultimately propagated as distance errors.
396 *
397 * @param clockOffset estimated receiver clock offset.
398 */
399 public void setClockOffset(final double clockOffset) {
400 this.clockOffset = clockOffset;
401 }
402
403 /**
404 * Gets estimated receiver clock drift expressed in meters per second (m/s).
405 * Notice that the rate at which clock errors increase or decrease will ultimately
406 * propagate as speed (and hence position) errors.
407 *
408 * @return estimated receiver clock drift.
409 */
410 public double getClockDrift() {
411 return clockDrift;
412 }
413
414 /**
415 * Sets estimated receiver clock drift expressed in meters per second (m/s).
416 * Notice that the rate at which clock errors increase or decrease will ultimately
417 * propagate as speed (and hence position) errors.
418 *
419 * @param clockDrift estimated receiver clock drift.
420 */
421 public void setClockDrift(final double clockDrift) {
422 this.clockDrift = clockDrift;
423 }
424
425 /**
426 * Gets x coordinate of estimated ECEF user position.
427 *
428 * @param result instance where x coordinate of estimated ECEF user position will be stored.
429 */
430 public void getDistanceX(final Distance result) {
431 result.setValue(x);
432 result.setUnit(DistanceUnit.METER);
433 }
434
435 /**
436 * Gets x coordinate of estimated ECEF user position.
437 *
438 * @return x coordinate of estimated ECEF user position.
439 */
440 public Distance getDistanceX() {
441 return new Distance(x, DistanceUnit.METER);
442 }
443
444 /**
445 * Sets x coordinate of estimated ECEF user position.
446 *
447 * @param x x coordinate of estimated ECEF user position.
448 */
449 public void setDistanceX(final Distance x) {
450 this.x = DistanceConverter.convert(x.getValue().doubleValue(), x.getUnit(), DistanceUnit.METER);
451 }
452
453 /**
454 * Gets y coordinate of estimated ECEF user position.
455 *
456 * @param result instance where y coordinate of estimated ECEF user position will be stored.
457 */
458 public void getDistanceY(final Distance result) {
459 result.setValue(y);
460 result.setUnit(DistanceUnit.METER);
461 }
462
463 /**
464 * Gets y coordinate of estimated ECEF user position.
465 *
466 * @return y coordinate of estimated ECEF user position.
467 */
468 public Distance getDistanceY() {
469 return new Distance(y, DistanceUnit.METER);
470 }
471
472 /**
473 * Sets y coordinate of estimated ECEF user position.
474 *
475 * @param y y coordinate of estimated ECEF user position.
476 */
477 public void setDistanceY(final Distance y) {
478 this.y = DistanceConverter.convert(y.getValue().doubleValue(), y.getUnit(), DistanceUnit.METER);
479 }
480
481 /**
482 * Gets z coordinate of estimated ECEF user position.
483 *
484 * @param result instance where z coordinate of estimated ECEF user position will be stored.
485 */
486 public void getDistanceZ(final Distance result) {
487 result.setValue(z);
488 result.setUnit(DistanceUnit.METER);
489 }
490
491 /**
492 * Gets z coordinate of estimated ECEF user position.
493 *
494 * @return z coordinate of estimated ECEF user position.
495 */
496 public Distance getDistanceZ() {
497 return new Distance(z, DistanceUnit.METER);
498 }
499
500 /**
501 * Sets z coordinate of estimated ECEF user position.
502 *
503 * @param z z coordinate of estimated ECEF user position.
504 */
505 public void setDistanceZ(final Distance z) {
506 this.z = DistanceConverter.convert(z.getValue().doubleValue(), z.getUnit(), DistanceUnit.METER);
507 }
508
509 /**
510 * Sets coordinates of estimated ECEF user position.
511 *
512 * @param x x coordinate.
513 * @param y y coordinate.
514 * @param z z coordinate.
515 */
516 public void setPositionCoordinates(final Distance x, final Distance y, final Distance z) {
517 setDistanceX(x);
518 setDistanceY(y);
519 setDistanceZ(z);
520 }
521
522 /**
523 * Gets x coordinate of estimated ECEF user velocity.
524 *
525 * @param result instance where x coordinate of estimated ECEF user velocity will
526 * be stored.
527 */
528 public void getSpeedX(final Speed result) {
529 result.setValue(vx);
530 result.setUnit(SpeedUnit.METERS_PER_SECOND);
531 }
532
533 /**
534 * Gets x coordinate of estimated ECEF user velocity.
535 *
536 * @return x coordinate of estimated ECEF user velocity.
537 */
538 public Speed getSpeedX() {
539 return new Speed(vx, SpeedUnit.METERS_PER_SECOND);
540 }
541
542 /**
543 * Sets x coordinate of estimated ECEF user velocity.
544 *
545 * @param speedX x coordinate of estimated ECEF user velocity.
546 */
547 public void setSpeedX(final Speed speedX) {
548 vx = SpeedConverter.convert(speedX.getValue().doubleValue(), speedX.getUnit(), SpeedUnit.METERS_PER_SECOND);
549 }
550
551 /**
552 * Gets y coordinate of estimated ECEF user velocity.
553 *
554 * @param result instance where y coordinate of estimated ECEF user velocity will
555 * be stored.
556 */
557 public void getSpeedY(final Speed result) {
558 result.setValue(vy);
559 result.setUnit(SpeedUnit.METERS_PER_SECOND);
560 }
561
562 /**
563 * Gets y coordinate of estimated ECEF user velocity.
564 *
565 * @return y coordinate of estimated ECEF user velocity.
566 */
567 public Speed getSpeedY() {
568 return new Speed(vy, SpeedUnit.METERS_PER_SECOND);
569 }
570
571 /**
572 * Sets y coordinate of estimated ECEF user velocity.
573 *
574 * @param speedY y coordinate of estimated ECEF user velocity.
575 */
576 public void setSpeedY(final Speed speedY) {
577 vy = SpeedConverter.convert(speedY.getValue().doubleValue(), speedY.getUnit(), SpeedUnit.METERS_PER_SECOND);
578 }
579
580 /**
581 * Gets z coordinate of estimated ECEF user velocity.
582 *
583 * @param result instance where z coordinate of estimated ECEF user velocity will
584 * be stored.
585 */
586 public void getSpeedZ(final Speed result) {
587 result.setValue(vz);
588 result.setUnit(SpeedUnit.METERS_PER_SECOND);
589 }
590
591 /**
592 * Gets z coordinate of estimated ECEF user velocity.
593 *
594 * @return z coordinate of estimated ECEF user velocity.
595 */
596 public Speed getSpeedZ() {
597 return new Speed(vz, SpeedUnit.METERS_PER_SECOND);
598 }
599
600 /**
601 * Sets z coordinate of estimated ECEF user velocity.
602 *
603 * @param speedZ z coordinate of estimated ECEF user velocity.
604 */
605 public void setSpeedZ(final Speed speedZ) {
606 vz = SpeedConverter.convert(speedZ.getValue().doubleValue(), speedZ.getUnit(), SpeedUnit.METERS_PER_SECOND);
607 }
608
609 /**
610 * Sets coordinates of estimated ECEF user velocity.
611 *
612 * @param speedX x coordinate.
613 * @param speedY y coordinate.
614 * @param speedZ z coordinate.
615 */
616 public void setVelocityCoordinates(final Speed speedX, final Speed speedY, final Speed speedZ) {
617 setSpeedX(speedX);
618 setSpeedY(speedY);
619 setSpeedZ(speedZ);
620 }
621
622 /**
623 * Gets estimated receiver clock offset.
624 * Notice that clock offset is estimated in terms of distance, since timing errors
625 * will ultimately be propagated as distance errors.
626 *
627 * @param result instance where estimated receiver clock offset will be stored.
628 */
629 public void getClockOffsetDistance(final Distance result) {
630 result.setValue(clockOffset);
631 result.setUnit(DistanceUnit.METER);
632 }
633
634 /**
635 * Gets estimated receiver clock offset.
636 * Notice that clock offset is estimated in terms of distance, since timing errors
637 * will ultimately be propagated as distance errors.
638 *
639 * @return estimated receiver clock offset.
640 */
641 public Distance getClockOffsetDistance() {
642 return new Distance(clockOffset, DistanceUnit.METER);
643 }
644
645 /**
646 * Sets estimated receiver clock offset.
647 * Notice that clock offset is estimated in terms of distance, since timing errors
648 * are ultimately propagated as distance errors.
649 *
650 * @param clockOffset estimated receiver clock offset.
651 */
652 public void setClockOffset(final Distance clockOffset) {
653 this.clockOffset = DistanceConverter.convert(clockOffset.getValue().doubleValue(), clockOffset.getUnit(),
654 DistanceUnit.METER);
655 }
656
657 /**
658 * Gets estimated receiver clock drift.
659 * Notice that the rate at which clock errors increase or decrease will ultimately
660 * propagate as speed (and hence position) errors.
661 *
662 * @param result instance where estimated receiver clock drift will be stored.
663 */
664 public void getClockDriftSpeed(final Speed result) {
665 result.setValue(clockDrift);
666 result.setUnit(SpeedUnit.METERS_PER_SECOND);
667 }
668
669 /**
670 * Gets estimated receiver clock drift.
671 * Notice that the rate at which clock errors increase or decrease will ultimately
672 * propagate as speed (and hence position) errors.
673 *
674 * @return estimated receiver clock drift.
675 */
676 public Speed getClockDriftSpeed() {
677 return new Speed(clockDrift, SpeedUnit.METERS_PER_SECOND);
678 }
679
680 /**
681 * Sets estimated receiver clock drift.
682 * Notice that the rate at which clock errors increase or decrease will ultimately
683 * propagate as speed (and hence position) errors.
684 *
685 * @param clockDrift estimated receiver clock drift.
686 */
687 public void setClockDrift(final Speed clockDrift) {
688 this.clockDrift = SpeedConverter.convert(clockDrift.getValue().doubleValue(), clockDrift.getUnit(),
689 SpeedUnit.METERS_PER_SECOND);
690 }
691
692 /**
693 * Gets estimated ECEF user position expressed in meters (m).
694 *
695 * @param result instance where estimated ECEF user position will be stored.
696 */
697 public void getPosition(final Point3D result) {
698 result.setInhomogeneousCoordinates(x, y, z);
699 }
700
701 /**
702 * Gets estimated ECEF user position expressed in meters (m).
703 *
704 * @return estimated ECEF user position.
705 */
706 public Point3D getPosition() {
707 return new InhomogeneousPoint3D(x, y, z);
708 }
709
710 /**
711 * Sets estimated ECEF user position expressed in meters (m).
712 *
713 * @param position estimated ECEF user position.
714 */
715 public void setPosition(final Point3D position) {
716 x = position.getInhomX();
717 y = position.getInhomY();
718 z = position.getInhomZ();
719 }
720
721 /**
722 * Gets estimatedECEF user position.
723 *
724 * @param result instance where result will be stored.
725 */
726 public void getEcefPosition(final ECEFPosition result) {
727 result.setCoordinates(x, y, z);
728 }
729
730 /**
731 * Gets estimated ECEF user position.
732 *
733 * @return estimated ECEF user position.
734 */
735 public ECEFPosition getEcefPosition() {
736 return new ECEFPosition(x, y, z);
737 }
738
739 /**
740 * Sets estimated ECEF user position.
741 *
742 * @param ecefPosition estimated ECEF user position.
743 */
744 public void setEcefPosition(final ECEFPosition ecefPosition) {
745 x = ecefPosition.getX();
746 y = ecefPosition.getY();
747 z = ecefPosition.getZ();
748 }
749
750 /**
751 * Gets estimated ECEF user velocity.
752 *
753 * @param result instance where result will be stored.
754 */
755 public void getEcefVelocity(final ECEFVelocity result) {
756 result.setCoordinates(vx, vy, vz);
757 }
758
759 /**
760 * Gets estimated ECEF user velocity.
761 *
762 * @return estimated ECEF user velocity.
763 */
764 public ECEFVelocity getEcefVelocity() {
765 return new ECEFVelocity(vx, vy, vz);
766 }
767
768 /**
769 * Sets estimated ECEF user velocity.
770 *
771 * @param ecefVelocity estimated ECEF user velocity.
772 */
773 public void setEcefVelocity(final ECEFVelocity ecefVelocity) {
774 vx = ecefVelocity.getVx();
775 vy = ecefVelocity.getVy();
776 vz = ecefVelocity.getVz();
777 }
778
779 /**
780 * Gets estimated ECEF user position and velocity.
781 *
782 * @param result instance where result will be stored.
783 */
784 public void getPositionAndVelocity(final ECEFPositionAndVelocity result) {
785 result.setPositionCoordinates(x, y, z);
786 result.setVelocityCoordinates(vx, vy, vz);
787 }
788
789 /**
790 * Gets estimated ECEF user position and velocity.
791 *
792 * @return estimated ECEF user position and velocity.
793 */
794 public ECEFPositionAndVelocity getPositionAndVelocity() {
795 return new ECEFPositionAndVelocity(x, y, z, vx, vy, vz);
796 }
797
798 /**
799 * Sets estimated ECEF user position and velocity.
800 *
801 * @param positionAndVelocity estimated ECEF user position and velocity.
802 */
803 public void setPositionAndVelocity(final ECEFPositionAndVelocity positionAndVelocity) {
804 setPositionCoordinates(positionAndVelocity.getX(), positionAndVelocity.getY(), positionAndVelocity.getZ());
805 setVelocityCoordinates(positionAndVelocity.getVx(), positionAndVelocity.getVy(), positionAndVelocity.getVz());
806 }
807
808 /**
809 * Converts state data into an array.
810 *
811 * @param result instance where state data will be stored.
812 * @throws IllegalArgumentException if provided array does not have length 8.
813 */
814 public void asArray(final double[] result) {
815 if (result.length != NUM_PARAMETERS) {
816 throw new IllegalArgumentException();
817 }
818
819 result[0] = x;
820 result[1] = y;
821 result[2] = z;
822 result[3] = vx;
823 result[4] = vy;
824 result[5] = vz;
825 result[6] = clockOffset;
826 result[7] = clockDrift;
827 }
828
829 /**
830 * Converts state data into an array.
831 *
832 * @return a new array containing state data.
833 */
834 public double[] asArray() {
835 final var result = new double[NUM_PARAMETERS];
836 asArray(result);
837 return result;
838 }
839
840 /**
841 * Sets array values into this instance state.
842 *
843 * @param array array to copy data from.
844 * @throws IllegalArgumentException if provided array does not have length 8.
845 */
846 public void fromArray(final double[] array) {
847 if (array.length != NUM_PARAMETERS) {
848 throw new IllegalArgumentException();
849 }
850
851 x = array[0];
852 y = array[1];
853 z = array[2];
854 vx = array[3];
855 vy = array[4];
856 vz = array[5];
857 clockOffset = array[6];
858 clockDrift = array[7];
859 }
860
861 /**
862 * Converts state data into a column matrix.
863 * If provided matrix is not 8x1 it will be resized.
864 *
865 * @param result instance where state data will be stored.
866 */
867 public void asMatrix(final Matrix result) {
868 if (result.getRows() != NUM_PARAMETERS || result.getColumns() != 1) {
869 try {
870 result.resize(NUM_PARAMETERS, 1);
871 } catch (WrongSizeException ignore) {
872 // never happens
873 }
874 }
875
876 result.setElementAtIndex(0, x);
877 result.setElementAtIndex(1, y);
878 result.setElementAtIndex(2, z);
879 result.setElementAtIndex(3, vx);
880 result.setElementAtIndex(4, vy);
881 result.setElementAtIndex(5, vz);
882 result.setElementAtIndex(6, clockOffset);
883 result.setElementAtIndex(7, clockDrift);
884 }
885
886 /**
887 * Converts state data into a column matrix.
888 *
889 * @return a new 8x1 column matrix containing state data.
890 */
891 public Matrix asMatrix() {
892 final Matrix result;
893 try {
894 result = new Matrix(NUM_PARAMETERS, 1);
895 asMatrix(result);
896 return result;
897 } catch (final WrongSizeException ignore) {
898 // never happens
899 return null;
900 }
901 }
902
903 /**
904 * Sets matrix values into this instance state.
905 *
906 * @param matrix matrix to copy data from.
907 * @throws IllegalArgumentException if provided matrix is not 8x1.
908 */
909 public void fromMatrix(final Matrix matrix) {
910 if (matrix.getRows() != NUM_PARAMETERS || matrix.getColumns() != 1) {
911 throw new IllegalArgumentException();
912 }
913 fromArray(matrix.getBuffer());
914 }
915
916 /**
917 * Copies this instance data into provided instance.
918 *
919 * @param output destination instance where data will be copied to.
920 */
921 public void copyTo(final GNSSEstimation output) {
922 output.x = x;
923 output.y = y;
924 output.z = z;
925
926 output.vx = vx;
927 output.vy = vy;
928 output.vz = vz;
929
930 output.clockOffset = clockOffset;
931 output.clockDrift = clockDrift;
932 }
933
934 /**
935 * Copies data of provided instance into this instance.
936 *
937 * @param input instance to copy data from.
938 */
939 public void copyFrom(final GNSSEstimation input) {
940 x = input.x;
941 y = input.y;
942 z = input.z;
943
944 vx = input.vx;
945 vy = input.vy;
946 vz = input.vz;
947
948 clockOffset = input.clockOffset;
949 clockDrift = input.clockDrift;
950 }
951
952 /**
953 * Computes and returns hash code for this instance. Hash codes are almost unique
954 * values that are useful for fast classification and storage of objects in collections.
955 *
956 * @return Hash code.
957 */
958 @Override
959 public int hashCode() {
960 return Objects.hash(x, y, z, vx, vy, vz, clockOffset, clockDrift);
961 }
962
963 /**
964 * Checks if provided object is a GNSSKalmanStateEstimates having exactly the same
965 * contents as this instance.
966 *
967 * @param obj Object to be compared.
968 * @return true if both objects are considered to be equal, false otherwise.
969 */
970 @Override
971 public boolean equals(final Object obj) {
972 if (obj == this) {
973 return true;
974 }
975 if (obj == null || getClass() != obj.getClass()) {
976 return false;
977 }
978
979 final GNSSEstimation other = (GNSSEstimation) obj;
980 return equals(other);
981 }
982
983 /**
984 * Checks if provided instance has exactly the same contents as this instance.
985 *
986 * @param other instance to be compared.
987 * @return true if both instances are considered to be equal, false otherwise.
988 */
989 public boolean equals(final GNSSEstimation other) {
990 return equals(other, 0.0);
991 }
992
993 /**
994 * Checks if provided instance has contents similar to this instance up to provided
995 * threshold value.
996 *
997 * @param other instance to be compared.
998 * @param threshold maximum difference allowed for values.
999 * @return true if both instances are considered to be equal (up to provided threshold),
1000 * false otherwise.
1001 */
1002 public boolean equals(final GNSSEstimation other, final double threshold) {
1003 if (other == null) {
1004 return false;
1005 }
1006
1007 return Math.abs(x - other.x) <= threshold
1008 && Math.abs(y - other.y) <= threshold
1009 && Math.abs(z - other.z) <= threshold
1010 && Math.abs(vx - other.vx) <= threshold
1011 && Math.abs(vy - other.vy) <= threshold
1012 && Math.abs(vz - other.vz) <= threshold
1013 && Math.abs(clockOffset - other.clockOffset) <= threshold
1014 && Math.abs(clockDrift - other.clockDrift) <= threshold;
1015 }
1016
1017 /**
1018 * Makes a copy of this instance.
1019 *
1020 * @return a copy of this instance.
1021 * @throws CloneNotSupportedException if clone fails for some reason.
1022 */
1023 @Override
1024 protected Object clone() throws CloneNotSupportedException {
1025 final var result = (GNSSEstimation) super.clone();
1026 copyTo(result);
1027 return result;
1028 }
1029 }