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.AlgebraException;
19 import com.irurueta.algebra.Matrix;
20 import com.irurueta.algebra.SingularValueDecomposer;
21 import com.irurueta.geometry.InhomogeneousPoint3D;
22 import com.irurueta.geometry.InvalidRotationMatrixException;
23 import com.irurueta.geometry.Point3D;
24 import com.irurueta.geometry.Rotation3D;
25 import com.irurueta.navigation.frames.CoordinateTransformation;
26 import com.irurueta.navigation.frames.ECEFFrame;
27 import com.irurueta.navigation.frames.ECEFPosition;
28 import com.irurueta.navigation.frames.ECEFVelocity;
29 import com.irurueta.navigation.frames.FrameType;
30 import com.irurueta.navigation.frames.InvalidSourceAndDestinationFrameTypeException;
31 import com.irurueta.navigation.gnss.ECEFPositionAndVelocity;
32 import com.irurueta.units.*;
33
34 import java.io.Serial;
35 import java.io.Serializable;
36 import java.util.Objects;
37
38 /**
39 * Kalman filter state for loosely coupled INS/GNSS extended kalman filter.
40 */
41 public class INSLooselyCoupledKalmanState implements Serializable, Cloneable {
42
43 /**
44 * Number of parameters of the Kalman filter.
45 */
46 public static final int NUM_PARAMS = 15;
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 * Estimated body to ECEF coordinate transformation matrix.
57 */
58 private Matrix bodyToEcefCoordinateTransformationMatrix;
59
60 /**
61 * Estimated ECEF user velocity resolved around x axis and expressed in meters per second (m/s).
62 */
63 private double vx;
64
65 /**
66 * Estimated ECEF user velocity resolved around y axis and expressed in meters per second (m/s).
67 */
68 private double vy;
69
70 /**
71 * Estimated ECEF user velocity resolved around z axis and expressed in meters per second (m/s).
72 */
73 private double vz;
74
75 /**
76 * X coordinate of estimated ECEF user position expressed in meters (m).
77 */
78 private double x;
79
80 /**
81 * Y coordinate of estimated ECEF user position expressed in meters (m).
82 */
83 private double y;
84
85 /**
86 * Z coordinate of estimated ECEF user position expressed in meters (m).
87 */
88 private double z;
89
90 /**
91 * Estimated accelerometer bias resolved around x axis and expressed in
92 * meters per squared second (m/s^2).
93 */
94 private double accelerationBiasX;
95
96 /**
97 * Estimated accelerometer bias resolved around y axis and expressed in
98 * meters per squared second (m/s^2).
99 */
100 private double accelerationBiasY;
101
102 /**
103 * Estimated accelerometer bias resolved around z axis and expressed in
104 * meters per squared second (m/s^2).
105 */
106 private double accelerationBiasZ;
107
108 /**
109 * Estimated gyroscope bias resolved around x axis and expressed in
110 * radians per second (rad/s).
111 */
112 private double gyroBiasX;
113
114 /**
115 * Estimated gyroscope bias resolved around y axis and expressed in
116 * radians per second (rad/s).
117 */
118 private double gyroBiasY;
119
120 /**
121 * Estimated gyroscope bias resolved around z axis and expressed in
122 * radians per second (rad/s).
123 */
124 private double gyroBiasZ;
125
126 /**
127 * Estimated Kalman filter error covariance matrix.
128 * Notice that covariance is expressed in terms of ECEF coordinates.
129 * If accuracy of position, attitude or velocity needs to be expressed in terms
130 * of NED coordinates, their respective sub-matrices of this covariance matrix
131 * must be rotated, taking into account the Jacobian of the matrix transformation
132 * relating both coordinates, the covariance can be expressed following the law
133 * of propagation of uncertainties
134 * <a href="https://en.wikipedia.org/wiki/Propagation_of_uncertainty">(https://en.wikipedia.org/wiki/Propagation_of_uncertainty)</a>
135 * as: cov(f(x)) = J*cov(x)*J'.
136 */
137 private Matrix covariance;
138
139 /**
140 * Constructor.
141 */
142 public INSLooselyCoupledKalmanState() {
143 }
144
145 /**
146 * Constructor.
147 *
148 * @param bodyToEcefCoordinateTransformationMatrix estimated body to ECEF coordinate transformation matrix.
149 * @param vx estimated ECEF user velocity resolved around x axis and
150 * expressed in meters per second (m/s).
151 * @param vy estimated ECEF user velocity resolved around y axis and
152 * expressed in meters per second (m/s).
153 * @param vz estimated ECEF user velocity resolved around z axis and
154 * expressed in meters per second (m/s).
155 * @param x x coordinate of estimated ECEF user position expressed
156 * in meters (m).
157 * @param y y coordinate of estimated ECEF user position expressed
158 * in meters (m).
159 * @param z z coordinate of estimated ECEF user position expressed
160 * in meters (m).
161 * @param accelerationBiasX estimated accelerometer bias resolved around x axis and
162 * expressed in meters per squared second (m/s^2).
163 * @param accelerationBiasY estimated accelerometer bias resolved around y axis and
164 * expressed in meters per squared second (m/s^2).
165 * @param accelerationBiasZ estimated accelerometer bias resolved around z axis and
166 * expressed in meters per squared second (m/s^2).
167 * @param gyroBiasX estimated gyroscope bias resolved around x axis and
168 * expressed in radians per second (rad/s).
169 * @param gyroBiasY estimated gyroscope bias resolved around y axis and
170 * expressed in radians per second (rad/s).
171 * @param gyroBiasZ estimated gyroscope bias resolved around z axis and
172 * expressed in radians per second (rad/s).
173 * @param covariance estimated Kalman filter error covariance matrix.
174 * @throws IllegalArgumentException if provided body to ECEF coordinate transformation matrix is not 3x3
175 * or if provided covariance matrix is not 15x15.
176 */
177 public INSLooselyCoupledKalmanState(
178 final Matrix bodyToEcefCoordinateTransformationMatrix, final double vx, final double vy, final double vz,
179 final double x, final double y, final double z,
180 final double accelerationBiasX, final double accelerationBiasY, final double accelerationBiasZ,
181 final double gyroBiasX, final double gyroBiasY, final double gyroBiasZ, final Matrix covariance) {
182 setBodyToEcefCoordinateTransformationMatrix(bodyToEcefCoordinateTransformationMatrix);
183 setVelocityCoordinates(vx, vy, vz);
184 setPositionCoordinates(x, y, z);
185 setAccelerationBiasCoordinates(accelerationBiasX, accelerationBiasY, accelerationBiasZ);
186 setGyroBiasCoordinates(gyroBiasX, gyroBiasY, gyroBiasZ);
187 setCovariance(covariance);
188 }
189
190 /**
191 * Constructor.
192 *
193 * @param c body to ECEF coordinate transformation.
194 * @param vx estimated ECEF user velocity resolved around x axis.
195 * @param vy estimated ECEF user velocity resolved around y axis.
196 * @param vz estimated ECEF user velocity resolved around z axis.
197 * @param x x coordinate of estimated ECEF user position.
198 * @param y y coordinate of estimated ECEF user position.
199 * @param z z coordinate of estimated ECEF user position.
200 * @param accelerationBiasX estimated accelerometer bias resolved around x axis and
201 * expressed in meters per squared second (m/s^2).
202 * @param accelerationBiasY estimated accelerometer bias resolved around y axis and
203 * expressed in meters per squared second (m/s^2).
204 * @param accelerationBiasZ estimated accelerometer bias resolved around z axis and
205 * expressed in meters per squared second (m/s^2).
206 * @param gyroBiasX estimated gyroscope bias resolved around x axis and
207 * expressed in radians per second (rad/s).
208 * @param gyroBiasY estimated gyroscope bias resolved around y axis and
209 * expressed in radians per second (rad/s).
210 * @param gyroBiasZ estimated gyroscope bias resolved around z axis and
211 * expressed in radians per second (rad/s).
212 * @param covariance estimated Kalman filter error covariance matrix.
213 * @throws IllegalArgumentException if provided covariance matrix is not 15x15.
214 */
215 public INSLooselyCoupledKalmanState(
216 final CoordinateTransformation c, final Speed vx, final Speed vy, final Speed vz,
217 final Distance x, final Distance y, final Distance z,
218 final double accelerationBiasX, final double accelerationBiasY, final double accelerationBiasZ,
219 final double gyroBiasX, final double gyroBiasY, final double gyroBiasZ, final Matrix covariance) {
220 setC(c);
221 setVelocityCoordinates(vx, vy, vz);
222 setPositionCoordinates(x, y, z);
223 setAccelerationBiasCoordinates(accelerationBiasX, accelerationBiasY, accelerationBiasZ);
224 setGyroBiasCoordinates(gyroBiasX, gyroBiasY, gyroBiasZ);
225 setCovariance(covariance);
226 }
227
228 /**
229 * Constructor.
230 *
231 * @param c body to ECEF coordinate transformation.
232 * @param vx estimated ECEF user velocity resolved around x axis.
233 * @param vy estimated ECEF user velocity resolved around y axis.
234 * @param vz estimated ECEF user velocity resolved around z axis.
235 * @param position estimated ECEF user position.
236 * @param accelerationBiasX estimated accelerometer bias resolved around x axis and
237 * expressed in meters per squared second (m/s^2).
238 * @param accelerationBiasY estimated accelerometer bias resolved around y axis and
239 * expressed in meters per squared second (m/s^2).
240 * @param accelerationBiasZ estimated accelerometer bias resolved around z axis and
241 * expressed in meters per squared second (m/s^2).
242 * @param gyroBiasX estimated gyroscope bias resolved around x axis and
243 * expressed in radians per second (rad/s).
244 * @param gyroBiasY estimated gyroscope bias resolved around y axis and
245 * expressed in radians per second (rad/s).
246 * @param gyroBiasZ estimated gyroscope bias resolved around z axis and
247 * expressed in radians per second (rad/s).
248 * @param covariance estimated Kalman filter error covariance matrix.
249 * @throws IllegalArgumentException if provided covariance matrix is not 15x15.
250 */
251 public INSLooselyCoupledKalmanState(
252 final CoordinateTransformation c, final Speed vx, final Speed vy, final Speed vz, final Point3D position,
253 final double accelerationBiasX, final double accelerationBiasY, final double accelerationBiasZ,
254 final double gyroBiasX, final double gyroBiasY, final double gyroBiasZ, final Matrix covariance) {
255 setC(c);
256 setVelocityCoordinates(vx, vy, vz);
257 setPosition(position);
258 setAccelerationBiasCoordinates(accelerationBiasX, accelerationBiasY, accelerationBiasZ);
259 setGyroBiasCoordinates(gyroBiasX, gyroBiasY, gyroBiasZ);
260 setCovariance(covariance);
261 }
262
263 /**
264 * Constructor.
265 *
266 * @param c body to ECEF coordinate transformation.
267 * @param velocity estimated ECEF user velocity.
268 * @param position estimated ECEF user position.
269 * @param accelerationBiasX estimated accelerometer bias resolved around x axis and
270 * expressed in meters per squared second (m/s^2).
271 * @param accelerationBiasY estimated accelerometer bias resolved around y axis and
272 * expressed in meters per squared second (m/s^2).
273 * @param accelerationBiasZ estimated accelerometer bias resolved around z axis and
274 * expressed in meters per squared second (m/s^2).
275 * @param gyroBiasX estimated gyroscope bias resolved around x axis and
276 * expressed in radians per second (rad/s).
277 * @param gyroBiasY estimated gyroscope bias resolved around y axis and
278 * expressed in radians per second (rad/s).
279 * @param gyroBiasZ estimated gyroscope bias resolved around z axis and
280 * expressed in radians per second (rad/s).
281 * @param covariance estimated Kalman filter error covariance matrix.
282 * @throws IllegalArgumentException if provided covariance matrix is not 15x15.
283 */
284 public INSLooselyCoupledKalmanState(
285 final CoordinateTransformation c, final ECEFVelocity velocity, final ECEFPosition position,
286 final double accelerationBiasX, final double accelerationBiasY, final double accelerationBiasZ,
287 final double gyroBiasX, final double gyroBiasY, final double gyroBiasZ, final Matrix covariance) {
288 setC(c);
289 setEcefVelocity(velocity);
290 setEcefPosition(position);
291 setAccelerationBiasCoordinates(accelerationBiasX, accelerationBiasY, accelerationBiasZ);
292 setGyroBiasCoordinates(gyroBiasX, gyroBiasY, gyroBiasZ);
293 setCovariance(covariance);
294 }
295
296 /**
297 * Constructor.
298 *
299 * @param c body to ECEF coordinate transformation.
300 * @param positionAndVelocity estimated ECEF user velocity and position.
301 * @param accelerationBiasX estimated accelerometer bias resolved around x axis and
302 * expressed in meters per squared second (m/s^2).
303 * @param accelerationBiasY estimated accelerometer bias resolved around y axis and
304 * expressed in meters per squared second (m/s^2).
305 * @param accelerationBiasZ estimated accelerometer bias resolved around z axis and
306 * expressed in meters per squared second (m/s^2).
307 * @param gyroBiasX estimated gyroscope bias resolved around x axis and
308 * expressed in radians per second (rad/s).
309 * @param gyroBiasY estimated gyroscope bias resolved around y axis and
310 * expressed in radians per second (rad/s).
311 * @param gyroBiasZ estimated gyroscope bias resolved around z axis and
312 * expressed in radians per second (rad/s).
313 * @param covariance estimated Kalman filter error covariance matrix.
314 * @throws IllegalArgumentException if provided covariance matrix is not 15x15.
315 */
316 public INSLooselyCoupledKalmanState(
317 final CoordinateTransformation c, final ECEFPositionAndVelocity positionAndVelocity,
318 final double accelerationBiasX, final double accelerationBiasY, final double accelerationBiasZ,
319 final double gyroBiasX, final double gyroBiasY, final double gyroBiasZ, final Matrix covariance) {
320 setC(c);
321 setPositionAndVelocity(positionAndVelocity);
322 setAccelerationBiasCoordinates(accelerationBiasX, accelerationBiasY, accelerationBiasZ);
323 setGyroBiasCoordinates(gyroBiasX, gyroBiasY, gyroBiasZ);
324 setCovariance(covariance);
325 }
326
327 /**
328 * Constructor.
329 *
330 * @param frame estimated user ECEF frame.
331 * @param accelerationBiasX estimated accelerometer bias resolved around x axis and
332 * expressed in meters per squared second (m/s^2).
333 * @param accelerationBiasY estimated accelerometer bias resolved around y axis and
334 * expressed in meters per squared second (m/s^2).
335 * @param accelerationBiasZ estimated accelerometer bias resolved around z axis and
336 * expressed in meters per squared second (m/s^2).
337 * @param gyroBiasX estimated gyroscope bias resolved around x axis and
338 * expressed in radians per second (rad/s).
339 * @param gyroBiasY estimated gyroscope bias resolved around y axis and
340 * expressed in radians per second (rad/s).
341 * @param gyroBiasZ estimated gyroscope bias resolved around z axis and
342 * expressed in radians per second (rad/s).
343 * @param covariance estimated Kalman filter error covariance .
344 * @throws IllegalArgumentException if provided covariance matrix is not 15x15.
345 */
346 public INSLooselyCoupledKalmanState(
347 final ECEFFrame frame,
348 final double accelerationBiasX, final double accelerationBiasY, final double accelerationBiasZ,
349 final double gyroBiasX, final double gyroBiasY, final double gyroBiasZ, final Matrix covariance) {
350 setFrame(frame);
351 setAccelerationBiasCoordinates(accelerationBiasX, accelerationBiasY, accelerationBiasZ);
352 setGyroBiasCoordinates(gyroBiasX, gyroBiasY, gyroBiasZ);
353 setCovariance(covariance);
354 }
355
356 /**
357 * Constructor.
358 *
359 * @param c body to ECEF coordinate transformation.
360 * @param vx estimated ECEF user velocity resolved around x axis.
361 * @param vy estimated ECEF user velocity resolved around y axis.
362 * @param vz estimated ECEF user velocity resolved around z axis.
363 * @param x x coordinate of estimated ECEF user position.
364 * @param y y coordinate of estimated ECEF user position.
365 * @param z z coordinate of estimated ECEF user position.
366 * @param accelerationBiasX estimated accelerometer bias resolved around x axis.
367 * @param accelerationBiasY estimated accelerometer bias resolved around y axis.
368 * @param accelerationBiasZ estimated accelerometer bias resolved around z axis.
369 * @param gyroBiasX estimated gyroscope bias resolved around x axis.
370 * @param gyroBiasY estimated gyroscope bias resolved around y axis.
371 * @param gyroBiasZ estimated gyroscope bias resolved around z axis.
372 * @param covariance estimated Kalman filter error covariance matrix.
373 * @throws IllegalArgumentException if provided covariance matrix is not 15x15.
374 */
375 public INSLooselyCoupledKalmanState(
376 final CoordinateTransformation c, final Speed vx, final Speed vy, final Speed vz,
377 final Distance x, final Distance y, final Distance z, final Acceleration accelerationBiasX,
378 final Acceleration accelerationBiasY, final Acceleration accelerationBiasZ,
379 final AngularSpeed gyroBiasX, final AngularSpeed gyroBiasY, final AngularSpeed gyroBiasZ,
380 final Matrix covariance) {
381 setC(c);
382 setVelocityCoordinates(vx, vy, vz);
383 setPositionCoordinates(x, y, z);
384 setAccelerationBiasCoordinates(accelerationBiasX, accelerationBiasY, accelerationBiasZ);
385 setGyroBiasCoordinates(gyroBiasX, gyroBiasY, gyroBiasZ);
386 setCovariance(covariance);
387 }
388
389 /**
390 * Constructor.
391 *
392 * @param c body to ECEF coordinate transformation.
393 * @param vx estimated ECEF user velocity resolved around x axis.
394 * @param vy estimated ECEF user velocity resolved around y axis.
395 * @param vz estimated ECEF user velocity resolved around z axis.
396 * @param position estimated ECEF user position expressed in meters (m).
397 * @param accelerationBiasX estimated accelerometer bias resolved around x axis.
398 * @param accelerationBiasY estimated accelerometer bias resolved around y axis.
399 * @param accelerationBiasZ estimated accelerometer bias resolved around z axis.
400 * @param gyroBiasX estimated gyroscope bias resolved around x axis.
401 * @param gyroBiasY estimated gyroscope bias resolved around y axis.
402 * @param gyroBiasZ estimated gyroscope bias resolved around z axis.
403 * @param covariance estimated Kalman filter error covariance matrix.
404 * @throws IllegalArgumentException if provided covariance matrix is not 15x15.
405 */
406 public INSLooselyCoupledKalmanState(
407 final CoordinateTransformation c, final Speed vx, final Speed vy, final Speed vz, final Point3D position,
408 final Acceleration accelerationBiasX, final Acceleration accelerationBiasY,
409 final Acceleration accelerationBiasZ, final AngularSpeed gyroBiasX, final AngularSpeed gyroBiasY,
410 final AngularSpeed gyroBiasZ, final Matrix covariance) {
411 setC(c);
412 setVelocityCoordinates(vx, vy, vz);
413 setPosition(position);
414 setAccelerationBiasCoordinates(accelerationBiasX, accelerationBiasY, accelerationBiasZ);
415 setGyroBiasCoordinates(gyroBiasX, gyroBiasY, gyroBiasZ);
416 setCovariance(covariance);
417 }
418
419 /**
420 * Constructor.
421 *
422 * @param c body to ECEF coordinate transformation.
423 * @param velocity estimated ECEF user velocity.
424 * @param position estimated ECEF user position.
425 * @param accelerationBiasX estimated accelerometer bias resolved around x axis.
426 * @param accelerationBiasY estimated accelerometer bias resolved around y axis.
427 * @param accelerationBiasZ estimated accelerometer bias resolved around z axis.
428 * @param gyroBiasX estimated gyroscope bias resolved around x axis.
429 * @param gyroBiasY estimated gyroscope bias resolved around y axis.
430 * @param gyroBiasZ estimated gyroscope bias resolved around z axis.
431 * @param covariance estimated Kalman filter error covariance matrix.
432 * @throws IllegalArgumentException if provided covariance matrix is not 15x15.
433 */
434 public INSLooselyCoupledKalmanState(
435 final CoordinateTransformation c, final ECEFVelocity velocity, final ECEFPosition position,
436 final Acceleration accelerationBiasX, final Acceleration accelerationBiasY,
437 final Acceleration accelerationBiasZ, final AngularSpeed gyroBiasX, final AngularSpeed gyroBiasY,
438 final AngularSpeed gyroBiasZ, final Matrix covariance) {
439 setC(c);
440 setEcefVelocity(velocity);
441 setEcefPosition(position);
442 setAccelerationBiasCoordinates(accelerationBiasX, accelerationBiasY, accelerationBiasZ);
443 setGyroBiasCoordinates(gyroBiasX, gyroBiasY, gyroBiasZ);
444 setCovariance(covariance);
445 }
446
447 /**
448 * Constructor.
449 *
450 * @param c body to ECEF coordinate transformation.
451 * @param positionAndVelocity estimated ECEF user position and velocity.
452 * @param accelerationBiasX estimated accelerometer bias resolved around x axis.
453 * @param accelerationBiasY estimated accelerometer bias resolved around y axis.
454 * @param accelerationBiasZ estimated accelerometer bias resolved around z axis.
455 * @param gyroBiasX estimated gyroscope bias resolved around x axis.
456 * @param gyroBiasY estimated gyroscope bias resolved around y axis.
457 * @param gyroBiasZ estimated gyroscope bias resolved around z axis.
458 * @param covariance estimated Kalman filter error covariance matrix.
459 * @throws IllegalArgumentException if provided covariance matrix is not 15x15.
460 */
461 public INSLooselyCoupledKalmanState(
462 final CoordinateTransformation c, final ECEFPositionAndVelocity positionAndVelocity,
463 final Acceleration accelerationBiasX, final Acceleration accelerationBiasY,
464 final Acceleration accelerationBiasZ, final AngularSpeed gyroBiasX, final AngularSpeed gyroBiasY,
465 final AngularSpeed gyroBiasZ, final Matrix covariance) {
466 setC(c);
467 setPositionAndVelocity(positionAndVelocity);
468 setAccelerationBiasCoordinates(accelerationBiasX, accelerationBiasY, accelerationBiasZ);
469 setGyroBiasCoordinates(gyroBiasX, gyroBiasY, gyroBiasZ);
470 setCovariance(covariance);
471 }
472
473 /**
474 * Constructor.
475 *
476 * @param frame estimated user ECEF frame.
477 * @param accelerationBiasX estimated accelerometer bias resolved around x axis.
478 * @param accelerationBiasY estimated accelerometer bias resolved around y axis.
479 * @param accelerationBiasZ estimated accelerometer bias resolved around z axis.
480 * @param gyroBiasX estimated gyroscope bias resolved around x axis.
481 * @param gyroBiasY estimated gyroscope bias resolved around y axis.
482 * @param gyroBiasZ estimated gyroscope bias resolved around z axis.
483 * @param covariance estimated Kalman filter error covariance matrix.
484 * @throws IllegalArgumentException if provided covariance matrix is not 15x15.
485 */
486 public INSLooselyCoupledKalmanState(
487 final ECEFFrame frame, final Acceleration accelerationBiasX, final Acceleration accelerationBiasY,
488 final Acceleration accelerationBiasZ, final AngularSpeed gyroBiasX, final AngularSpeed gyroBiasY,
489 final AngularSpeed gyroBiasZ, final Matrix covariance) {
490 setFrame(frame);
491 setAccelerationBiasCoordinates(accelerationBiasX, accelerationBiasY, accelerationBiasZ);
492 setGyroBiasCoordinates(gyroBiasX, gyroBiasY, gyroBiasZ);
493 setCovariance(covariance);
494 }
495
496 /**
497 * Constructor.
498 *
499 * @param bodyToEcefCoordinateTransformationMatrix estimated body to ECEF coordinate transformation matrix.
500 * @param vx estimated ECEF user velocity resolved around x axis.
501 * @param vy estimated ECEF user velocity resolved around y axis.
502 * @param vz estimated ECEF user velocity resolved around z axis.
503 * @param x x coordinate of estimated ECEF user position.
504 * @param y y coordinate of estimated ECEF user position.
505 * @param z z coordinate of estimated ECEF user position.
506 * @param accelerationBiasX estimated accelerometer bias resolved around x axis and
507 * expressed in meters per squared second (m/s^2).
508 * @param accelerationBiasY estimated accelerometer bias resolved around y axis and
509 * expressed in meters per squared second (m/s^2).
510 * @param accelerationBiasZ estimated accelerometer bias resolved around z axis and
511 * expressed in meters per squared second (m/s^2).
512 * @param gyroBiasX estimated gyroscope bias resolved around x axis and
513 * expressed in radians per second (rad/s).
514 * @param gyroBiasY estimated gyroscope bias resolved around y axis and
515 * expressed in radians per second (rad/s).
516 * @param gyroBiasZ estimated gyroscope bias resolved around z axis and
517 * expressed in radians per second (rad/s).
518 * @param covariance estimated Kalman filter error covariance matrix.
519 * @throws IllegalArgumentException if provided body to ECEF coordinate transformation matrix is not 3x3
520 * or if provided covariance matrix is not 15x15.
521 */
522 public INSLooselyCoupledKalmanState(
523 final Matrix bodyToEcefCoordinateTransformationMatrix, final Speed vx, final Speed vy, final Speed vz,
524 final Distance x, final Distance y, final Distance z,
525 final double accelerationBiasX, final double accelerationBiasY, final double accelerationBiasZ,
526 final double gyroBiasX, final double gyroBiasY, final double gyroBiasZ, final Matrix covariance) {
527 setBodyToEcefCoordinateTransformationMatrix(bodyToEcefCoordinateTransformationMatrix);
528 setVelocityCoordinates(vx, vy, vz);
529 setPositionCoordinates(x, y, z);
530 setAccelerationBiasCoordinates(accelerationBiasX, accelerationBiasY, accelerationBiasZ);
531 setGyroBiasCoordinates(gyroBiasX, gyroBiasY, gyroBiasZ);
532 setCovariance(covariance);
533 }
534
535 /**
536 * Constructor.
537 *
538 * @param bodyToEcefCoordinateTransformationMatrix estimated body to ECEF coordinate transformation matrix.
539 * @param vx estimated ECEF user velocity resolved around x axis.
540 * @param vy estimated ECEF user velocity resolved around y axis.
541 * @param vz estimated ECEF user velocity resolved around z axis.
542 * @param position estimated ECEF user position.
543 * @param accelerationBiasX estimated accelerometer bias resolved around x axis and
544 * expressed in meters per squared second (m/s^2).
545 * @param accelerationBiasY estimated accelerometer bias resolved around y axis and
546 * expressed in meters per squared second (m/s^2).
547 * @param accelerationBiasZ estimated accelerometer bias resolved around z axis and
548 * expressed in meters per squared second (m/s^2).
549 * @param gyroBiasX estimated gyroscope bias resolved around x axis and
550 * expressed in radians per second (rad/s).
551 * @param gyroBiasY estimated gyroscope bias resolved around y axis and
552 * expressed in radians per second (rad/s).
553 * @param gyroBiasZ estimated gyroscope bias resolved around z axis and
554 * expressed in radians per second (rad/s).
555 * @param covariance estimated Kalman filter error covariance matrix.
556 * @throws IllegalArgumentException if provided body to ECEF coordinate transformation matrix is not 3x3
557 * or if provided covariance matrix is not 15x15.
558 */
559 public INSLooselyCoupledKalmanState(
560 final Matrix bodyToEcefCoordinateTransformationMatrix, final Speed vx, final Speed vy, final Speed vz,
561 final Point3D position,
562 final double accelerationBiasX, final double accelerationBiasY, final double accelerationBiasZ,
563 final double gyroBiasX, final double gyroBiasY, final double gyroBiasZ, final Matrix covariance) {
564 setBodyToEcefCoordinateTransformationMatrix(bodyToEcefCoordinateTransformationMatrix);
565 setVelocityCoordinates(vx, vy, vz);
566 setPosition(position);
567 setAccelerationBiasCoordinates(accelerationBiasX, accelerationBiasY, accelerationBiasZ);
568 setGyroBiasCoordinates(gyroBiasX, gyroBiasY, gyroBiasZ);
569 setCovariance(covariance);
570 }
571
572 /**
573 * Constructor.
574 *
575 * @param bodyToEcefCoordinateTransformationMatrix estimated body to ECEF coordinate transformation matrix.
576 * @param velocity estimated ECEF user velocity.
577 * @param position estimated ECEF user position.
578 * @param accelerationBiasX estimated accelerometer bias resolved around x axis and
579 * expressed in meters per squared second (m/s^2).
580 * @param accelerationBiasY estimated accelerometer bias resolved around y axis and
581 * expressed in meters per squared second (m/s^2).
582 * @param accelerationBiasZ estimated accelerometer bias resolved around z axis and
583 * expressed in meters per squared second (m/s^2).
584 * @param gyroBiasX estimated gyroscope bias resolved around x axis and
585 * expressed in radians per second (rad/s).
586 * @param gyroBiasY estimated gyroscope bias resolved around y axis and
587 * expressed in radians per second (rad/s).
588 * @param gyroBiasZ estimated gyroscope bias resolved around z axis and
589 * expressed in radians per second (rad/s).
590 * @param covariance estimated Kalman filter error covariance matrix.
591 * @throws IllegalArgumentException if provided body to ECEF coordinate transformation matrix is not 3x3
592 * or if provided covariance matrix is not 15x15.
593 */
594 public INSLooselyCoupledKalmanState(
595 final Matrix bodyToEcefCoordinateTransformationMatrix, final ECEFVelocity velocity,
596 final ECEFPosition position,
597 final double accelerationBiasX, final double accelerationBiasY, final double accelerationBiasZ,
598 final double gyroBiasX, final double gyroBiasY, final double gyroBiasZ, final Matrix covariance) {
599 setBodyToEcefCoordinateTransformationMatrix(bodyToEcefCoordinateTransformationMatrix);
600 setEcefVelocity(velocity);
601 setEcefPosition(position);
602 setAccelerationBiasCoordinates(accelerationBiasX, accelerationBiasY, accelerationBiasZ);
603 setGyroBiasCoordinates(gyroBiasX, gyroBiasY, gyroBiasZ);
604 setCovariance(covariance);
605 }
606
607 /**
608 * Constructor.
609 *
610 * @param bodyToEcefCoordinateTransformationMatrix estimated body to ECEF coordinate transformation matrix.
611 * @param positionAndVelocity estimated ECEF user position and velocity.
612 * @param accelerationBiasX estimated accelerometer bias resolved around x axis and
613 * expressed in meters per squared second (m/s^2).
614 * @param accelerationBiasY estimated accelerometer bias resolved around y axis and
615 * expressed in meters per squared second (m/s^2).
616 * @param accelerationBiasZ estimated accelerometer bias resolved around z axis and
617 * expressed in meters per squared second (m/s^2).
618 * @param gyroBiasX estimated gyroscope bias resolved around x axis and
619 * expressed in radians per second (rad/s).
620 * @param gyroBiasY estimated gyroscope bias resolved around y axis and
621 * expressed in radians per second (rad/s).
622 * @param gyroBiasZ estimated gyroscope bias resolved around z axis and
623 * expressed in radians per second (rad/s).
624 * @param covariance estimated Kalman filter error covariance matrix.
625 * @throws IllegalArgumentException if provided body to ECEF coordinate transformation matrix is not 3x3
626 * or if provided covariance matrix is not 15x15.
627 */
628 public INSLooselyCoupledKalmanState(
629 final Matrix bodyToEcefCoordinateTransformationMatrix, final ECEFPositionAndVelocity positionAndVelocity,
630 final double accelerationBiasX, final double accelerationBiasY, final double accelerationBiasZ,
631 final double gyroBiasX, final double gyroBiasY, final double gyroBiasZ, final Matrix covariance) {
632 setBodyToEcefCoordinateTransformationMatrix(bodyToEcefCoordinateTransformationMatrix);
633 setPositionAndVelocity(positionAndVelocity);
634 setAccelerationBiasCoordinates(accelerationBiasX, accelerationBiasY, accelerationBiasZ);
635 setGyroBiasCoordinates(gyroBiasX, gyroBiasY, gyroBiasZ);
636 setCovariance(covariance);
637 }
638
639 /**
640 * Constructor.
641 *
642 * @param bodyToEcefCoordinateTransformationMatrix estimated body to ECEF coordinate transformation matrix.
643 * @param vx estimated ECEF user velocity resolved around x axis.
644 * @param vy estimated ECEF user velocity resolved around y axis.
645 * @param vz estimated ECEF user velocity resolved around z axis.
646 * @param x x coordinate of estimated ECEF user position.
647 * @param y y coordinate of estimated ECEF user position.
648 * @param z z coordinate of estimated ECEF user position.
649 * @param accelerationBiasX estimated accelerometer bias resolved around x axis.
650 * @param accelerationBiasY estimated accelerometer bias resolved around y axis.
651 * @param accelerationBiasZ estimated accelerometer bias resolved around z axis.
652 * @param gyroBiasX estimated gyroscope bias resolved around x axis.
653 * @param gyroBiasY estimated gyroscope bias resolved around y axis.
654 * @param gyroBiasZ estimated gyroscope bias resolved around z axis.
655 * @param covariance estimated Kalman filter error covariance matrix.
656 * @throws IllegalArgumentException if provided body to ECEF coordinate transformation matrix is not 3x3
657 * or if provided covariance matrix is not 15x15.
658 */
659 public INSLooselyCoupledKalmanState(
660 final Matrix bodyToEcefCoordinateTransformationMatrix, final Speed vx, final Speed vy, final Speed vz,
661 final Distance x, final Distance y, final Distance z, final Acceleration accelerationBiasX,
662 final Acceleration accelerationBiasY, final Acceleration accelerationBiasZ, final AngularSpeed gyroBiasX,
663 final AngularSpeed gyroBiasY, final AngularSpeed gyroBiasZ, final Matrix covariance) {
664 setBodyToEcefCoordinateTransformationMatrix(bodyToEcefCoordinateTransformationMatrix);
665 setVelocityCoordinates(vx, vy, vz);
666 setPositionCoordinates(x, y, z);
667 setAccelerationBiasCoordinates(accelerationBiasX, accelerationBiasY, accelerationBiasZ);
668 setGyroBiasCoordinates(gyroBiasX, gyroBiasY, gyroBiasZ);
669 setCovariance(covariance);
670 }
671
672 /**
673 * Constructor.
674 *
675 * @param bodyToEcefCoordinateTransformationMatrix estimated body to ECEF coordinate transformation matrix.
676 * @param vx estimated ECEF user velocity resolved around x axis.
677 * @param vy estimated ECEF user velocity resolved around y axis.
678 * @param vz estimated ECEF user velocity resolved around z axis.
679 * @param position estimated ECEF user position expressed in meters (m).
680 * @param accelerationBiasX estimated accelerometer bias resolved around x axis.
681 * @param accelerationBiasY estimated accelerometer bias resolved around y axis.
682 * @param accelerationBiasZ estimated accelerometer bias resolved around z axis.
683 * @param gyroBiasX estimated gyroscope bias resolved around x axis.
684 * @param gyroBiasY estimated gyroscope bias resolved around y axis.
685 * @param gyroBiasZ estimated gyroscope bias resolved around z axis.
686 * @param covariance estimated Kalman filter error covariance matrix.
687 * @throws IllegalArgumentException if provided body to ECEF coordinate transformation matrix is not 3x3
688 * or if provided covariance matrix is not 15x15.
689 */
690 public INSLooselyCoupledKalmanState(
691 final Matrix bodyToEcefCoordinateTransformationMatrix, final Speed vx, final Speed vy, final Speed vz,
692 final Point3D position, final Acceleration accelerationBiasX, final Acceleration accelerationBiasY,
693 final Acceleration accelerationBiasZ, final AngularSpeed gyroBiasX, final AngularSpeed gyroBiasY,
694 final AngularSpeed gyroBiasZ, final Matrix covariance) {
695 setBodyToEcefCoordinateTransformationMatrix(bodyToEcefCoordinateTransformationMatrix);
696 setVelocityCoordinates(vx, vy, vz);
697 setPosition(position);
698 setAccelerationBiasCoordinates(accelerationBiasX, accelerationBiasY, accelerationBiasZ);
699 setGyroBiasCoordinates(gyroBiasX, gyroBiasY, gyroBiasZ);
700 setCovariance(covariance);
701 }
702
703 /**
704 * Constructor.
705 *
706 * @param bodyToEcefCoordinateTransformationMatrix estimated body to ECEF coordinate transformation matrix.
707 * @param velocity estimated ECEF user velocity.
708 * @param position estimated ECEF user position.
709 * @param accelerationBiasX estimated accelerometer bias resolved around x axis.
710 * @param accelerationBiasY estimated accelerometer bias resolved around y axis.
711 * @param accelerationBiasZ estimated accelerometer bias resolved around z axis.
712 * @param gyroBiasX estimated gyroscope bias resolved around x axis.
713 * @param gyroBiasY estimated gyroscope bias resolved around y axis.
714 * @param gyroBiasZ estimated gyroscope bias resolved around z axis.
715 * @param covariance estimated Kalman filter error covariance matrix.
716 * @throws IllegalArgumentException if provided body to ECEF coordinate transformation matrix is not 3x3
717 * or if provided covariance matrix is not 15x15.
718 */
719 public INSLooselyCoupledKalmanState(
720 final Matrix bodyToEcefCoordinateTransformationMatrix, final ECEFVelocity velocity,
721 final ECEFPosition position, final Acceleration accelerationBiasX, final Acceleration accelerationBiasY,
722 final Acceleration accelerationBiasZ, final AngularSpeed gyroBiasX, final AngularSpeed gyroBiasY,
723 final AngularSpeed gyroBiasZ, final Matrix covariance) {
724 setBodyToEcefCoordinateTransformationMatrix(bodyToEcefCoordinateTransformationMatrix);
725 setEcefVelocity(velocity);
726 setEcefPosition(position);
727 setAccelerationBiasCoordinates(accelerationBiasX, accelerationBiasY, accelerationBiasZ);
728 setGyroBiasCoordinates(gyroBiasX, gyroBiasY, gyroBiasZ);
729 setCovariance(covariance);
730 }
731
732 /**
733 * Constructor.
734 *
735 * @param bodyToEcefCoordinateTransformationMatrix estimated body to ECEF coordinate transformation matrix.
736 * @param positionAndVelocity estimated ECEF user position and velocity.
737 * @param accelerationBiasX estimated accelerometer bias resolved around x axis.
738 * @param accelerationBiasY estimated accelerometer bias resolved around y axis.
739 * @param accelerationBiasZ estimated accelerometer bias resolved around z axis.
740 * @param gyroBiasX estimated gyroscope bias resolved around x axis.
741 * @param gyroBiasY estimated gyroscope bias resolved around y axis.
742 * @param gyroBiasZ estimated gyroscope bias resolved around z axis.
743 * @param covariance estimated Kalman filter error covariance matrix.
744 * @throws IllegalArgumentException if provided body to ECEF coordinate transformation matrix is not 3x3
745 * or if provided covariance matrix is not 15x15.
746 */
747 public INSLooselyCoupledKalmanState(
748 final Matrix bodyToEcefCoordinateTransformationMatrix, final ECEFPositionAndVelocity positionAndVelocity,
749 final Acceleration accelerationBiasX, final Acceleration accelerationBiasY,
750 final Acceleration accelerationBiasZ, final AngularSpeed gyroBiasX, final AngularSpeed gyroBiasY,
751 final AngularSpeed gyroBiasZ, final Matrix covariance) {
752 setBodyToEcefCoordinateTransformationMatrix(bodyToEcefCoordinateTransformationMatrix);
753 setPositionAndVelocity(positionAndVelocity);
754 setAccelerationBiasCoordinates(accelerationBiasX, accelerationBiasY, accelerationBiasZ);
755 setGyroBiasCoordinates(gyroBiasX, gyroBiasY, gyroBiasZ);
756 setCovariance(covariance);
757 }
758
759 /**
760 * Copy constructor.
761 *
762 * @param input input instance to copy data from.
763 */
764 public INSLooselyCoupledKalmanState(final INSLooselyCoupledKalmanState input) {
765 copyFrom(input);
766 }
767
768 /**
769 * Gets estimated body to ECEF coordinate transformation matrix.
770 *
771 * @return estimated body to ECEF coordinate transformation matrix.
772 */
773 public Matrix getBodyToEcefCoordinateTransformationMatrix() {
774 return bodyToEcefCoordinateTransformationMatrix;
775 }
776
777 /**
778 * Sets estimated body to ECEF coordinate transformation matrix.
779 *
780 * @param bodyToEcefCoordinateTransformationMatrix estimated body to ECEF coordinate
781 * transformation matrix.
782 * @throws IllegalArgumentException if provided matrix is not 3x3.
783 */
784 public void setBodyToEcefCoordinateTransformationMatrix(final Matrix bodyToEcefCoordinateTransformationMatrix) {
785 if (bodyToEcefCoordinateTransformationMatrix.getRows() != CoordinateTransformation.ROWS
786 || bodyToEcefCoordinateTransformationMatrix.getColumns() != CoordinateTransformation.COLS) {
787 throw new IllegalArgumentException();
788 }
789 this.bodyToEcefCoordinateTransformationMatrix = bodyToEcefCoordinateTransformationMatrix;
790 }
791
792 /**
793 * Gets estimated ECEF user velocity resolved around x axis and expressed in meters per second (m/s).
794 *
795 * @return estimated ECEF user velocity resolved around x axis and expressed in meters per second (m/s).
796 */
797 public double getVx() {
798 return vx;
799 }
800
801 /**
802 * Sets estimated ECEF user velocity resolved around x axis and expressed in meters per second (m/s).
803 *
804 * @param vx estimated ECEF user velocity resolved around x axis and expressed in meters per second (m/s).
805 */
806 public void setVx(final double vx) {
807 this.vx = vx;
808 }
809
810 /**
811 * Gets estimated ECEF user velocity resolved around y axis and expressed in meters per second (m/s).
812 *
813 * @return estimated ECEF user velocity resolved around y axis and expressed in meters per second (m/s).
814 */
815 public double getVy() {
816 return vy;
817 }
818
819 /**
820 * Sets estimated ECEF user velocity resolved around y axis and expressed in meters per second (m/s).
821 *
822 * @param vy estimated ECEF user velocity resolved around y axis and expressed in meters per second (m/s).
823 */
824 public void setVy(final double vy) {
825 this.vy = vy;
826 }
827
828 /**
829 * Gets estimated ECEF user velocity resolved around z axis and expressed in meters per second (m/s).
830 *
831 * @return estimated ECEF user velocity resolved around z axis and expressed in meters per second (m/s).
832 */
833 public double getVz() {
834 return vz;
835 }
836
837 /**
838 * Sets estimated ECEF user velocity resolved around z axis and expressed in meters per second (m/s).
839 *
840 * @param vz estimated ECEF user velocity resolved around z axis and expressed in meters per second (m/s).
841 */
842 public void setVz(final double vz) {
843 this.vz = vz;
844 }
845
846 /**
847 * Sets estimated ECEF user velocity coordinates.
848 *
849 * @param vx estimated ECEF user velocity resolved around x axis and expressed in meters per second (m/s).
850 * @param vy estimated ECEF user velocity resolved around y axis and expressed in meters per second (m/s).
851 * @param vz estimated ECEF user velocity resolved around z axis and expressed in meters per second (m/s).
852 */
853 public void setVelocityCoordinates(final double vx, final double vy, final double vz) {
854 this.vx = vx;
855 this.vy = vy;
856 this.vz = vz;
857 }
858
859 /**
860 * Gets x coordinate of estimated ECEF user position expressed in meters (m).
861 *
862 * @return x coordinate of estimated ECEF user position expressed in meters (m).
863 */
864 public double getX() {
865 return x;
866 }
867
868 /**
869 * Sets x coordinate of estimated ECEF user position expressed in meters (m).
870 *
871 * @param x x coordinate of estimated ECEF user position expressed in meters (m).
872 */
873 public void setX(final double x) {
874 this.x = x;
875 }
876
877 /**
878 * Gets y coordinate of estimated ECEF user position expressed in meters (m).
879 *
880 * @return y coordinate of estimated ECEF user position expressed in meters (m).
881 */
882 public double getY() {
883 return y;
884 }
885
886 /**
887 * Sets y coordinate of estimated ECEF user position expressed in meters (m).
888 *
889 * @param y y coordinate of estimated ECEF user position expressed in meters (m).
890 */
891 public void setY(final double y) {
892 this.y = y;
893 }
894
895 /**
896 * Gets z coordinate of estimated ECEF user position expressed in meters (m).
897 *
898 * @return z coordinate of estimated ECEF user position expressed in meters (m).
899 */
900 public double getZ() {
901 return z;
902 }
903
904 /**
905 * Sets z coordinate of estimated ECEF user position expressed in meters (m).
906 *
907 * @param z z coordinate of estimated ECEF user position expressed in meters (m).
908 */
909 public void setZ(final double z) {
910 this.z = z;
911 }
912
913 /**
914 * Sets estimated ECEF user position coordinates.
915 *
916 * @param x x coordinate of estimated ECEF user position expressed in meters (m).
917 * @param y y coordinate of estimated ECEF user position expressed in meters (m).
918 * @param z z coordinate of estimated ECEF user position expressed in meters (m).
919 */
920 public void setPositionCoordinates(final double x, final double y, final double z) {
921 this.x = x;
922 this.y = y;
923 this.z = z;
924 }
925
926 /**
927 * Gets estimated accelerometer bias resolved around x axis and expressed in
928 * meters per squared second (m/s^2).
929 *
930 * @return estimated accelerometer bias resolved around x axis and expressed in
931 * meters per squared second (m/s^2).
932 */
933 public double getAccelerationBiasX() {
934 return accelerationBiasX;
935 }
936
937 /**
938 * Sets estimated accelerometer bias resolved around x axis and expressed in
939 * meters per squared second (m/s^2).
940 *
941 * @param accelerationBiasX estimated accelerometer bias resolved around x axis and
942 * expressed in meters per squared second (m/s^2).
943 */
944 public void setAccelerationBiasX(final double accelerationBiasX) {
945 this.accelerationBiasX = accelerationBiasX;
946 }
947
948 /**
949 * Gets estimated accelerometer bias resolved around y axis and expressed in
950 * meters per squared second (m/s^2).
951 *
952 * @return estimated accelerometer bias resolved around y axis and expressed in
953 * meters per squared second (m/s^2).
954 */
955 public double getAccelerationBiasY() {
956 return accelerationBiasY;
957 }
958
959 /**
960 * Sets estimated accelerometer bias resolved around y axis and expressed in
961 * meters per squared second (m/s^2).
962 *
963 * @param accelerationBiasY estimated accelerometer bias resolved around y axis
964 * and expressed in meters per squared second (m/s^2).
965 */
966 public void setAccelerationBiasY(final double accelerationBiasY) {
967 this.accelerationBiasY = accelerationBiasY;
968 }
969
970 /**
971 * Gets estimated accelerometer bias resolved around z axis and expressed in
972 * meters per squared second (m/s^2).
973 *
974 * @return estimated accelerometer bias resolved around z axis and
975 * expressed in meters per squared second (m/s^2).
976 */
977 public double getAccelerationBiasZ() {
978 return accelerationBiasZ;
979 }
980
981 /**
982 * Sets estimated accelerometer bias resolved around z axis and expressed in
983 * meters per squared second (m/s^2).
984 *
985 * @param accelerationBiasZ estimated accelerometer bias resolved around z axis
986 * and expressed in meters per squared second (m/s^2).
987 */
988 public void setAccelerationBiasZ(final double accelerationBiasZ) {
989 this.accelerationBiasZ = accelerationBiasZ;
990 }
991
992 /**
993 * Sets estimated accelerometer bias expressed in meters per squared second (m/s^2).
994 *
995 * @param accelerationBiasX estimated accelerometer bias resolved around x axis and
996 * expressed in meters per squared second (m/s^2).
997 * @param accelerationBiasY estimated accelerometer bias resolved around y axis and
998 * expressed in meters per squared second (m/s^2).
999 * @param accelerationBiasZ estimated accelerometer bias resolved around z axis and
1000 * expressed in meters per squared second (m/s^2).
1001 */
1002 public void setAccelerationBiasCoordinates(
1003 final double accelerationBiasX, final double accelerationBiasY, final double accelerationBiasZ) {
1004 this.accelerationBiasX = accelerationBiasX;
1005 this.accelerationBiasY = accelerationBiasY;
1006 this.accelerationBiasZ = accelerationBiasZ;
1007 }
1008
1009 /**
1010 * Gets estimated gyroscope bias resolved around x axis and expressed in
1011 * radians per second (rad/s).
1012 *
1013 * @return estimated gyroscope bias resolved around x axis and expressed in
1014 * radians per second (rad/s).
1015 */
1016 public double getGyroBiasX() {
1017 return gyroBiasX;
1018 }
1019
1020 /**
1021 * Sets estimated gyroscope bias resolved around x axis and expressed in
1022 * radians per second (rad/s).
1023 *
1024 * @param gyroBiasX estimated gyroscope bias resolved around x axis and
1025 * expressed in radians per second (rad/s).
1026 */
1027 public void setGyroBiasX(final double gyroBiasX) {
1028 this.gyroBiasX = gyroBiasX;
1029 }
1030
1031 /**
1032 * Gets estimated gyroscope bias resolved around y axis and expressed in
1033 * radians per second (rad/s).
1034 *
1035 * @return estimated gyroscope bias resolved around y axis and expressed
1036 * in radians per second (rad/s).
1037 */
1038 public double getGyroBiasY() {
1039 return gyroBiasY;
1040 }
1041
1042 /**
1043 * Sets estimated gyroscope bias resolved around y axis and expressed in
1044 * radians per second (rad/s).
1045 *
1046 * @param gyroBiasY estimated gyroscope bias resolved around y axis and
1047 * expressed in radians per second (rad/s).
1048 */
1049 public void setGyroBiasY(final double gyroBiasY) {
1050 this.gyroBiasY = gyroBiasY;
1051 }
1052
1053 /**
1054 * Gets estimated gyroscope bias resolved around z axis and expressed in
1055 * radians per second (rad/s).
1056 *
1057 * @return estimated gyroscope bias resolved around z axis and expressed
1058 * in radians per second (rad/s).
1059 */
1060 public double getGyroBiasZ() {
1061 return gyroBiasZ;
1062 }
1063
1064 /**
1065 * Sets estimated gyroscope bias resolved around z axis and expressed in
1066 * radians per second (rad/s).
1067 *
1068 * @param gyroBiasZ estimated gyroscope bias resolved around z axis and
1069 * expressed in radians per second (rad/s).
1070 */
1071 public void setGyroBiasZ(final double gyroBiasZ) {
1072 this.gyroBiasZ = gyroBiasZ;
1073 }
1074
1075 /**
1076 * Sets estimated gyroscope bias coordinates expressed in radians
1077 * per second (rad/s).
1078 *
1079 * @param gyroBiasX estimated gyroscope bias resolved around x axis and
1080 * expressed in radians per second (rad/s).
1081 * @param gyroBiasY estimated gyroscope bias resolved around y axis and
1082 * expressed in radians per second (rad/s).
1083 * @param gyroBiasZ estimated gyroscope bias resolved around z axis and
1084 * expressed in radians per second (rad/s).
1085 */
1086 public void setGyroBiasCoordinates(final double gyroBiasX, final double gyroBiasY, final double gyroBiasZ) {
1087 this.gyroBiasX = gyroBiasX;
1088 this.gyroBiasY = gyroBiasY;
1089 this.gyroBiasZ = gyroBiasZ;
1090 }
1091
1092 /**
1093 * Gets Kalman filter error covariance matrix.
1094 * Notice that covariance is expressed in terms of ECEF coordinates.
1095 * If accuracy of position, attitude or velocity needs to be expressed in terms
1096 * of NED coordinates, their respective sub-matrices of this covariance matrix
1097 * must be rotated, taking into account the Jacobian of the matrix transformation
1098 * relating both coordinates, the covariance can be expressed following the law
1099 * of propagation of uncertainties
1100 * <a href="https://en.wikipedia.org/wiki/Propagation_of_uncertainty">(https://en.wikipedia.org/wiki/Propagation_of_uncertainty)</a>
1101 * as: cov(f(x)) = J*cov(x)*J'.
1102 *
1103 * @param result instance where result data will be copied to.
1104 * @return true if result data has been copied, false otherwise.
1105 */
1106 public boolean getCovariance(final Matrix result) {
1107 if (covariance != null) {
1108 covariance.copyTo(result);
1109 return true;
1110 } else {
1111 return false;
1112 }
1113 }
1114
1115 /**
1116 * Gets Kalman filter error covariance matrix.
1117 * Notice that covariance is expressed in terms of ECEF coordinates.
1118 * If accuracy of position, attitude or velocity needs to be expressed in terms
1119 * of NED coordinates, their respective sub-matrices of this covariance matrix
1120 * must be rotated, taking into account the Jacobian of the matrix transformation
1121 * relating both coordinates, the covariance can be expressed following the law
1122 * of propagation of uncertainties
1123 * <a href="https://en.wikipedia.org/wiki/Propagation_of_uncertainty">(https://en.wikipedia.org/wiki/Propagation_of_uncertainty)</a>
1124 * as: cov(f(x)) = J*cov(x)*J'.
1125 *
1126 * @return Kalman filter error covariance matrix.
1127 */
1128 public Matrix getCovariance() {
1129 return covariance;
1130 }
1131
1132 /**
1133 * Sets Kalman filter error covariance matrix.
1134 *
1135 * @param covariance Kalman filter error covariance matrix to be set.
1136 * @throws IllegalArgumentException if provided covariance matrix is not 15x15.
1137 */
1138 public void setCovariance(final Matrix covariance) {
1139 if (covariance.getRows() != NUM_PARAMS || covariance.getColumns() != NUM_PARAMS) {
1140 throw new IllegalArgumentException();
1141 }
1142
1143 this.covariance = covariance;
1144 }
1145
1146 /**
1147 * Gets body to ECEF coordinate transformation.
1148 *
1149 * @return body to ECEF coordinate transformation.
1150 * @throws InvalidRotationMatrixException if current body to ECEF transformation matrix is
1151 * not valid (is not a 3x3 orthonormal matrix).
1152 */
1153 public CoordinateTransformation getC() throws InvalidRotationMatrixException {
1154 if (bodyToEcefCoordinateTransformationMatrix != null) {
1155 try {
1156 // Make sure that matrix is orthonormal
1157 final var fixed = fixRotationMatrix();
1158 return new CoordinateTransformation(fixed, FrameType.BODY_FRAME,
1159 FrameType.EARTH_CENTERED_EARTH_FIXED_FRAME);
1160 } catch (final AlgebraException ignore) {
1161 return null;
1162 }
1163
1164 } else {
1165 return null;
1166 }
1167 }
1168
1169 /**
1170 * Gets body to ECEF coordinate transformation.
1171 *
1172 * @param threshold threshold to determine whether current body to ECEF transformation
1173 * matrix is valid or not (to check that matrix is 3x3 orthonormal).
1174 * @return body to ECEF coordinate transformation.
1175 * @throws InvalidRotationMatrixException if current body to ECEF transformation matrix
1176 * is considered not valid (is not a 3x3 orthonormal matrix) with provided threshold.
1177 */
1178 public CoordinateTransformation getC(final double threshold) throws InvalidRotationMatrixException {
1179 return bodyToEcefCoordinateTransformationMatrix != null
1180 ? new CoordinateTransformation(bodyToEcefCoordinateTransformationMatrix, FrameType.BODY_FRAME,
1181 FrameType.EARTH_CENTERED_EARTH_FIXED_FRAME, threshold)
1182 : null;
1183 }
1184
1185 /**
1186 * Gets body to ECEF coordinate transformation.
1187 *
1188 * @param result instance where body to ECEF coordinate transformation will be stored.
1189 * @return true if result instance was updated, false otherwise.
1190 * @throws InvalidRotationMatrixException if current body to ECEF transformation matrix
1191 * is not valid (is not a 3x3 orthonormal matrix).
1192 */
1193 public boolean getC(final CoordinateTransformation result) throws InvalidRotationMatrixException {
1194 if (bodyToEcefCoordinateTransformationMatrix != null) {
1195 result.setSourceType(FrameType.BODY_FRAME);
1196 result.setDestinationType(FrameType.EARTH_CENTERED_EARTH_FIXED_FRAME);
1197 result.setMatrix(bodyToEcefCoordinateTransformationMatrix);
1198 return true;
1199 } else {
1200 return false;
1201 }
1202 }
1203
1204 /**
1205 * Gets body to ECEF coordinate transformation.
1206 *
1207 * @param result instance where body to ECEF coordinate transformation will be stored.
1208 * @param threshold threshold to determine whether current body to ECEF transformation
1209 * matrix is valid or not (to check that matrix is 3x3 orthonormal).
1210 * @return true if result instance was updated, false otherwise.
1211 * @throws InvalidRotationMatrixException if current body to ECEF transformation matrix
1212 * is not valid (is not a 3x3 orthonormal matrix) with provided threshold.
1213 */
1214 public boolean getC(final CoordinateTransformation result, final double threshold)
1215 throws InvalidRotationMatrixException {
1216 if (bodyToEcefCoordinateTransformationMatrix != null) {
1217 result.setSourceType(FrameType.BODY_FRAME);
1218 result.setDestinationType(FrameType.EARTH_CENTERED_EARTH_FIXED_FRAME);
1219 result.setMatrix(bodyToEcefCoordinateTransformationMatrix, threshold);
1220 return true;
1221 } else {
1222 return false;
1223 }
1224 }
1225
1226 /**
1227 * Sets body to ECEF coordinate transformation.
1228 *
1229 * @param c body to ECEF coordinate transformation to be set.
1230 * @throws IllegalArgumentException if provided coordinate transformation is
1231 * not null and is not a body to ECEF transformation.
1232 */
1233 public void setC(final CoordinateTransformation c) {
1234 if (c == null) {
1235 bodyToEcefCoordinateTransformationMatrix = null;
1236
1237 } else {
1238
1239 if (c.getSourceType() != FrameType.BODY_FRAME
1240 || c.getDestinationType() != FrameType.EARTH_CENTERED_EARTH_FIXED_FRAME) {
1241 throw new IllegalArgumentException();
1242 }
1243
1244 if (bodyToEcefCoordinateTransformationMatrix != null) {
1245 c.getMatrix(bodyToEcefCoordinateTransformationMatrix);
1246 } else {
1247 bodyToEcefCoordinateTransformationMatrix = c.getMatrix();
1248 }
1249 }
1250 }
1251
1252 /**
1253 * Gets estimated ECEF user velocity resolved around x axis.
1254 *
1255 * @param result instance where estimated ECEF user velocity resolved around x axis will be stored.
1256 */
1257 public void getSpeedX(final Speed result) {
1258 result.setValue(vx);
1259 result.setUnit(SpeedUnit.METERS_PER_SECOND);
1260 }
1261
1262 /**
1263 * Gets estimated ECEF user velocity resolved around x axis.
1264 *
1265 * @return estimated ECEF user velocity resolved around x axis.
1266 */
1267 public Speed getSpeedX() {
1268 return new Speed(vx, SpeedUnit.METERS_PER_SECOND);
1269 }
1270
1271 /**
1272 * Sets estimated ECEF user velocity resolved around x axis.
1273 *
1274 * @param vx estimated ECEF user velocity resolved around x axis.
1275 */
1276 public void setSpeedX(final Speed vx) {
1277 this.vx = SpeedConverter.convert(vx.getValue().doubleValue(), vx.getUnit(), SpeedUnit.METERS_PER_SECOND);
1278 }
1279
1280 /**
1281 * Gets estimated ECEF user velocity resolved around y axis.
1282 *
1283 * @param result instance where estimated ECEF user velocity resolved around y axis will be stored.
1284 */
1285 public void getSpeedY(final Speed result) {
1286 result.setValue(vy);
1287 result.setUnit(SpeedUnit.METERS_PER_SECOND);
1288 }
1289
1290 /**
1291 * Gets estimated ECEF user velocity resolved around y axis.
1292 *
1293 * @return estimated ECEF velocity resolved around y axis.
1294 */
1295 public Speed getSpeedY() {
1296 return new Speed(vy, SpeedUnit.METERS_PER_SECOND);
1297 }
1298
1299 /**
1300 * Sets estimated ECEF user velocity resolved around y axis.
1301 *
1302 * @param vy estimated ECEF user velocity resolved around y axis.
1303 */
1304 public void setSpeedY(final Speed vy) {
1305 this.vy = SpeedConverter.convert(vy.getValue().doubleValue(), vy.getUnit(), SpeedUnit.METERS_PER_SECOND);
1306 }
1307
1308 /**
1309 * Gets estimated ECEF user velocity resolved around z axis.
1310 *
1311 * @param result instance where estimated ECEF user velocity resolved around z axis will be stored.
1312 */
1313 public void getSpeedZ(final Speed result) {
1314 result.setValue(vz);
1315 result.setUnit(SpeedUnit.METERS_PER_SECOND);
1316 }
1317
1318 /**
1319 * Gets estimated ECEF user velocity resolved around z axis.
1320 *
1321 * @return estimated ECEF velocity resolved around z axis.
1322 */
1323 public Speed getSpeedZ() {
1324 return new Speed(vz, SpeedUnit.METERS_PER_SECOND);
1325 }
1326
1327 /**
1328 * Sets estimated ECEF user velocity resolved around z axis.
1329 *
1330 * @param vz estimated ECEF velocity resolved around z axis.
1331 */
1332 public void setSpeedZ(final Speed vz) {
1333 this.vz = SpeedConverter.convert(vz.getValue().doubleValue(), vz.getUnit(), SpeedUnit.METERS_PER_SECOND);
1334 }
1335
1336 /**
1337 * Sets estimated ECEF user velocity.
1338 *
1339 * @param vx estimated ECEF velocity resolved around x axis.
1340 * @param vy estimated ECEF velocity resolved around y axis.
1341 * @param vz estimated ECEF velocity resolved around z axis.
1342 */
1343 public void setVelocityCoordinates(final Speed vx, final Speed vy, final Speed vz) {
1344 setSpeedX(vx);
1345 setSpeedY(vy);
1346 setSpeedZ(vz);
1347 }
1348
1349 /**
1350 * Gets estimated ECEF user velocity.
1351 *
1352 * @param result instance where estimated ECEF user velocity will be stored.
1353 */
1354 public void getEcefVelocity(final ECEFVelocity result) {
1355 result.setCoordinates(vx, vy, vz);
1356 }
1357
1358 /**
1359 * Gets estimated ECEF user velocity.
1360 *
1361 * @return estimated ECEF user velocity.
1362 */
1363 public ECEFVelocity getEcefVelocity() {
1364 return new ECEFVelocity(vx, vy, vz);
1365 }
1366
1367 /**
1368 * Sets estimated ECEF user velocity.
1369 *
1370 * @param ecefVelocity estimated ECEF user velocity.
1371 */
1372 public void setEcefVelocity(final ECEFVelocity ecefVelocity) {
1373 vx = ecefVelocity.getVx();
1374 vy = ecefVelocity.getVy();
1375 vz = ecefVelocity.getVz();
1376 }
1377
1378 /**
1379 * Gets x coordinate of estimated ECEF user position.
1380 *
1381 * @param result instance where x coordinate of estimated ECEF user position
1382 * will be stored.
1383 */
1384 public void getDistanceX(final Distance result) {
1385 result.setValue(x);
1386 result.setUnit(DistanceUnit.METER);
1387 }
1388
1389 /**
1390 * Gets x coordinate of estimated ECEF user position.
1391 *
1392 * @return x coordinate of estimated ECEF user position.
1393 */
1394 public Distance getDistanceX() {
1395 return new Distance(x, DistanceUnit.METER);
1396 }
1397
1398 /**
1399 * Sets x coordinate of estimated ECEF user position.
1400 *
1401 * @param x x coordinate of estimated ECEF user position.
1402 */
1403 public void setDistanceX(final Distance x) {
1404 this.x = DistanceConverter.convert(x.getValue().doubleValue(), x.getUnit(), DistanceUnit.METER);
1405 }
1406
1407 /**
1408 * Gets y coordinate of estimated ECEF user position.
1409 *
1410 * @param result instance where y coordinate of estimated ECEF user position
1411 * will be stored.
1412 */
1413 public void getDistanceY(final Distance result) {
1414 result.setValue(y);
1415 result.setUnit(DistanceUnit.METER);
1416 }
1417
1418 /**
1419 * Gets y coordinate of estimated ECEF user position.
1420 *
1421 * @return y coordinate of estimated ECEF user position.
1422 */
1423 public Distance getDistanceY() {
1424 return new Distance(y, DistanceUnit.METER);
1425 }
1426
1427 /**
1428 * Sets y coordinate of estimated ECEF user position.
1429 *
1430 * @param y y coordinate of estimated ECEF user position.
1431 */
1432 public void setDistanceY(final Distance y) {
1433 this.y = DistanceConverter.convert(y.getValue().doubleValue(), y.getUnit(), DistanceUnit.METER);
1434 }
1435
1436 /**
1437 * Gets z coordinate of estimated ECEF user position.
1438 *
1439 * @param result instance where z coordinate of estimated ECEF user position
1440 * will be stored.
1441 */
1442 public void getDistanceZ(final Distance result) {
1443 result.setValue(z);
1444 result.setUnit(DistanceUnit.METER);
1445 }
1446
1447 /**
1448 * Gets z coordinate of estimated ECEF user position.
1449 *
1450 * @return z coordinate of estimated ECEF user position.
1451 */
1452 public Distance getDistanceZ() {
1453 return new Distance(z, DistanceUnit.METER);
1454 }
1455
1456 /**
1457 * Sets z coordinate of estimated ECEF user position.
1458 *
1459 * @param z z coordinate of estimated ECEF user position.
1460 */
1461 public void setDistanceZ(final Distance z) {
1462 this.z = DistanceConverter.convert(z.getValue().doubleValue(), z.getUnit(), DistanceUnit.METER);
1463 }
1464
1465 /**
1466 * Sets coordinates of estimated ECEF user position.
1467 *
1468 * @param x x coordinate of estimated ECEF user position.
1469 * @param y y coordinate of estimated ECEF user position.
1470 * @param z z coordinate of estimated ECEF user position.
1471 */
1472 public void setPositionCoordinates(final Distance x, final Distance y, final Distance z) {
1473 setDistanceX(x);
1474 setDistanceY(y);
1475 setDistanceZ(z);
1476 }
1477
1478 /**
1479 * Gets estimated ECEF user position expressed in meters (m).
1480 *
1481 * @param result instance where estimated ECEF user position expressed
1482 * in meters (m) will be stored.
1483 */
1484 public void getPosition(final Point3D result) {
1485 result.setInhomogeneousCoordinates(x, y, z);
1486 }
1487
1488 /**
1489 * Gets estimated ECEF user position expressed in meters (m).
1490 *
1491 * @return estimated ECEF user position expressed in meters (m).
1492 */
1493 public Point3D getPosition() {
1494 return new InhomogeneousPoint3D(x, y, z);
1495 }
1496
1497 /**
1498 * Sets estimated ECEF user position expressed in meters (m).
1499 *
1500 * @param position estimated ECEF user position expressed in
1501 * meters (m).
1502 */
1503 public void setPosition(final Point3D position) {
1504 x = position.getInhomX();
1505 y = position.getInhomY();
1506 z = position.getInhomZ();
1507 }
1508
1509 /**
1510 * Gets estimated ECEF user position.
1511 *
1512 * @param result instance where estimated ECEF user position
1513 * will be stored.
1514 */
1515 public void getEcefPosition(final ECEFPosition result) {
1516 result.setCoordinates(x, y, z);
1517 }
1518
1519 /**
1520 * Gets estimated ECEF user position.
1521 *
1522 * @return estimated ECEF user position.
1523 */
1524 public ECEFPosition getEcefPosition() {
1525 return new ECEFPosition(x, y, z);
1526 }
1527
1528 /**
1529 * Sets estimated ECEF user position.
1530 *
1531 * @param ecefPosition estimated ECEF user position.
1532 */
1533 public void setEcefPosition(final ECEFPosition ecefPosition) {
1534 x = ecefPosition.getX();
1535 y = ecefPosition.getY();
1536 z = ecefPosition.getZ();
1537 }
1538
1539 /**
1540 * Gets estimated ECEF user position and velocity.
1541 *
1542 * @param result instance where estimated ECEF user position and velocity
1543 * will be stored.
1544 */
1545 public void getPositionAndVelocity(final ECEFPositionAndVelocity result) {
1546 result.setPositionCoordinates(x, y, z);
1547 result.setVelocityCoordinates(vx, vy, vz);
1548 }
1549
1550 /**
1551 * Gets estimated ECEF user position and velocity.
1552 *
1553 * @return estimated ECEF user position and velocity.
1554 */
1555 public ECEFPositionAndVelocity getPositionAndVelocity() {
1556 return new ECEFPositionAndVelocity(x, y, z, vx, vy, vz);
1557 }
1558
1559 /**
1560 * Sets estimated ECEF user position and velocity.
1561 *
1562 * @param positionAndVelocity estimated ECEF user position and velocity.
1563 */
1564 public void setPositionAndVelocity(final ECEFPositionAndVelocity positionAndVelocity) {
1565 x = positionAndVelocity.getX();
1566 y = positionAndVelocity.getY();
1567 z = positionAndVelocity.getZ();
1568 vx = positionAndVelocity.getVx();
1569 vy = positionAndVelocity.getVy();
1570 vz = positionAndVelocity.getVz();
1571 }
1572
1573 /**
1574 * Gets body to ECEF frame containing coordinate transformation, position and
1575 * velocity.
1576 *
1577 * @param result instance where body to ECEF frame will be stored.
1578 * @return true if result was updated, false otherwise.
1579 */
1580 public boolean getFrame(final ECEFFrame result) {
1581 if (bodyToEcefCoordinateTransformationMatrix != null) {
1582 try {
1583 result.setCoordinateTransformation(getC());
1584 } catch (final InvalidSourceAndDestinationFrameTypeException | InvalidRotationMatrixException e) {
1585 return false;
1586 }
1587 result.setCoordinates(x, y, z);
1588 result.setVelocityCoordinates(vx, vy, vz);
1589 return true;
1590 } else {
1591 return false;
1592 }
1593 }
1594
1595 /**
1596 * Gets body to ECEF frame containing coordinate transformation, position and
1597 * velocity.
1598 *
1599 * @return body to ECEF frame.
1600 */
1601 public ECEFFrame getFrame() {
1602 if (bodyToEcefCoordinateTransformationMatrix != null) {
1603 try {
1604 return new ECEFFrame(x, y, z, vx, vy, vz, getC());
1605 } catch (final InvalidSourceAndDestinationFrameTypeException | InvalidRotationMatrixException e) {
1606 return null;
1607 }
1608 } else {
1609 return null;
1610 }
1611 }
1612
1613 /**
1614 * Sets body to ECEF frame containing coordinate transformation, position and
1615 * velocity.
1616 *
1617 * @param frame body to ECEF frame to be set.
1618 */
1619 public void setFrame(final ECEFFrame frame) {
1620 x = frame.getX();
1621 y = frame.getY();
1622 z = frame.getZ();
1623
1624 vx = frame.getVx();
1625 vy = frame.getVy();
1626 vz = frame.getVz();
1627
1628 if (bodyToEcefCoordinateTransformationMatrix != null) {
1629 frame.getCoordinateTransformationMatrix(bodyToEcefCoordinateTransformationMatrix);
1630 } else {
1631 bodyToEcefCoordinateTransformationMatrix = frame.getCoordinateTransformationMatrix();
1632 }
1633 }
1634
1635 /**
1636 * Gets estimated accelerometer bias resolved around x axis.
1637 *
1638 * @param result instance where estimated accelerometer bias resolved around
1639 * x axis will be stored.
1640 */
1641 public void getAccelerationBiasXAsAcceleration(final Acceleration result) {
1642 result.setValue(accelerationBiasX);
1643 result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
1644 }
1645
1646 /**
1647 * Gets estimated accelerometer bias resolved around x axis.
1648 *
1649 * @return estimated accelerometer bias resolved around x axis.
1650 */
1651 public Acceleration getAccelerationBiasXAsAcceleration() {
1652 return new Acceleration(accelerationBiasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
1653 }
1654
1655 /**
1656 * Sets estimated accelerometer bias resolved around x axis.
1657 *
1658 * @param accelerationBiasX estimated accelerometer bias resolved
1659 * around x axis.
1660 */
1661 public void setAccelerationBiasX(final Acceleration accelerationBiasX) {
1662 this.accelerationBiasX = AccelerationConverter.convert(accelerationBiasX.getValue().doubleValue(),
1663 accelerationBiasX.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1664 }
1665
1666 /**
1667 * Gets estimated accelerometer bias resolved around y axis.
1668 *
1669 * @param result instance where estimated accelerometer bias resolved around
1670 * y axis will be stored.
1671 */
1672 public void getAccelerationBiasYAsAcceleration(final Acceleration result) {
1673 result.setValue(accelerationBiasY);
1674 result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
1675 }
1676
1677 /**
1678 * Gets estimated accelerometer bias resolved around y axis.
1679 *
1680 * @return estimated accelerometer bias resolved around y axis.
1681 */
1682 public Acceleration getAccelerationBiasYAsAcceleration() {
1683 return new Acceleration(accelerationBiasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
1684 }
1685
1686 /**
1687 * Sets estimated accelerometer bias resolved around y axis.
1688 *
1689 * @param accelerationBiasY estimated accelerometer bias resolved
1690 * around y axis.
1691 */
1692 public void setAccelerationBiasY(final Acceleration accelerationBiasY) {
1693 this.accelerationBiasY = AccelerationConverter.convert(accelerationBiasY.getValue().doubleValue(),
1694 accelerationBiasY.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1695 }
1696
1697 /**
1698 * Gets estimated accelerometer bias resolved around z axis.
1699 *
1700 * @param result instance where estimated accelerometer bias resolved around
1701 * z axis will be stored.
1702 */
1703 public void getAccelerationBiasZAsAcceleration(final Acceleration result) {
1704 result.setValue(accelerationBiasZ);
1705 result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
1706 }
1707
1708 /**
1709 * Gets estimated accelerometer bias resolved around z axis.
1710 *
1711 * @return estimated accelerometer bias resolved around z axis.
1712 */
1713 public Acceleration getAccelerationBiasZAsAcceleration() {
1714 return new Acceleration(accelerationBiasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
1715 }
1716
1717 /**
1718 * Sets estimated accelerometer bias resolved around z axis.
1719 *
1720 * @param accelerationBiasZ estimated accelerometer bias resolved
1721 * around z axis.
1722 */
1723 public void setAccelerationBiasZ(final Acceleration accelerationBiasZ) {
1724 this.accelerationBiasZ = AccelerationConverter.convert(accelerationBiasZ.getValue().doubleValue(),
1725 accelerationBiasZ.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1726 }
1727
1728 /**
1729 * Sets estimated accelerometer bias coordinates.
1730 *
1731 * @param accelerationBiasX estimated accelerometer bias resolved around x axis.
1732 * @param accelerationBiasY estimated accelerometer bias resolved around y axis.
1733 * @param accelerationBiasZ estimated accelerometer bias resolved around z axis.
1734 */
1735 public void setAccelerationBiasCoordinates(
1736 final Acceleration accelerationBiasX, final Acceleration accelerationBiasY,
1737 final Acceleration accelerationBiasZ) {
1738 setAccelerationBiasX(accelerationBiasX);
1739 setAccelerationBiasY(accelerationBiasY);
1740 setAccelerationBiasZ(accelerationBiasZ);
1741 }
1742
1743 /**
1744 * Gets estimated gyroscope bias resolved around x axis.
1745 *
1746 * @param result instance where estimated gyroscope bias resolved around x axis will
1747 * be stored.
1748 */
1749 public void getAngularSpeedGyroBiasX(final AngularSpeed result) {
1750 result.setValue(gyroBiasX);
1751 result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
1752 }
1753
1754 /**
1755 * Gets estimated gyroscope bias resolved around x axis.
1756 *
1757 * @return estimated gyroscope bias resolved around x axis.
1758 */
1759 public AngularSpeed getAngularSpeedGyroBiasX() {
1760 return new AngularSpeed(gyroBiasX, AngularSpeedUnit.RADIANS_PER_SECOND);
1761 }
1762
1763 /**
1764 * Sets estimated gyroscope bias resolved around x axis.
1765 *
1766 * @param gyroBiasX estimated gyroscope bias resolved around x axis.
1767 */
1768 public void setGyroBiasX(final AngularSpeed gyroBiasX) {
1769 this.gyroBiasX = AngularSpeedConverter.convert(gyroBiasX.getValue().doubleValue(), gyroBiasX.getUnit(),
1770 AngularSpeedUnit.RADIANS_PER_SECOND);
1771 }
1772
1773 /**
1774 * Gets estimated gyroscope bias resolved around y axis.
1775 *
1776 * @param result instance where estimated gyroscope bias resolved around y axis will
1777 * be stored.
1778 */
1779 public void getAngularSpeedGyroBiasY(final AngularSpeed result) {
1780 result.setValue(gyroBiasY);
1781 result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
1782 }
1783
1784 /**
1785 * Gets estimated gyroscope bias resolved around y axis.
1786 *
1787 * @return estimated gyroscope bias resolved around y axis.
1788 */
1789 public AngularSpeed getAngularSpeedGyroBiasY() {
1790 return new AngularSpeed(gyroBiasY, AngularSpeedUnit.RADIANS_PER_SECOND);
1791 }
1792
1793 /**
1794 * Sets estimated gyroscope bias resolved around y axis.
1795 *
1796 * @param gyroBiasY estimated gyroscope bias resolved around y axis.
1797 */
1798 public void setGyroBiasY(final AngularSpeed gyroBiasY) {
1799 this.gyroBiasY = AngularSpeedConverter.convert(gyroBiasY.getValue().doubleValue(), gyroBiasY.getUnit(),
1800 AngularSpeedUnit.RADIANS_PER_SECOND);
1801 }
1802
1803 /**
1804 * Gets estimated gyroscope bias resolved around z axis.
1805 *
1806 * @param result instance where estimated gyroscope bias resolved around z axis will
1807 * be stored.
1808 */
1809 public void getAngularSpeedGyroBiasZ(final AngularSpeed result) {
1810 result.setValue(gyroBiasZ);
1811 result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
1812 }
1813
1814 /**
1815 * Gets estimated gyroscope bias resolved around z axis.
1816 *
1817 * @return estimated gyroscope bias resolved around z axis.
1818 */
1819 public AngularSpeed getAngularSpeedGyroBiasZ() {
1820 return new AngularSpeed(gyroBiasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
1821 }
1822
1823 /**
1824 * Sets estimated gyroscope bias resolved around z axis.
1825 *
1826 * @param gyroBiasZ estimated gyroscope bias resolved around z axis.
1827 */
1828 public void setGyroBiasZ(final AngularSpeed gyroBiasZ) {
1829 this.gyroBiasZ = AngularSpeedConverter.convert(gyroBiasZ.getValue().doubleValue(), gyroBiasZ.getUnit(),
1830 AngularSpeedUnit.RADIANS_PER_SECOND);
1831 }
1832
1833 /**
1834 * Sets estimated gyroscope bias coordinates.
1835 *
1836 * @param gyroBiasX estimated gyroscope bias resolved around x axis.
1837 * @param gyroBiasY estimated gyroscope bias resolved around y axis.
1838 * @param gyroBiasZ estimated gyroscope bias resolved around z axis.
1839 */
1840 public void setGyroBiasCoordinates(
1841 final AngularSpeed gyroBiasX, final AngularSpeed gyroBiasY, final AngularSpeed gyroBiasZ) {
1842 setGyroBiasX(gyroBiasX);
1843 setGyroBiasY(gyroBiasY);
1844 setGyroBiasZ(gyroBiasZ);
1845 }
1846
1847 /**
1848 * Copies this instance data into provided instance.
1849 *
1850 * @param output destination instance where data will be copied to.
1851 */
1852 public void copyTo(final INSLooselyCoupledKalmanState output) {
1853 output.copyFrom(this);
1854 }
1855
1856 /**
1857 * Copies data of provided instance into this instance.
1858 *
1859 * @param input instance to copy data from.
1860 */
1861 public void copyFrom(final INSLooselyCoupledKalmanState input) {
1862 // copy coordinate transformation matrix
1863 if (input.bodyToEcefCoordinateTransformationMatrix == null) {
1864 bodyToEcefCoordinateTransformationMatrix = null;
1865 } else {
1866 if (bodyToEcefCoordinateTransformationMatrix == null) {
1867 bodyToEcefCoordinateTransformationMatrix = new Matrix(input.bodyToEcefCoordinateTransformationMatrix);
1868 } else {
1869 bodyToEcefCoordinateTransformationMatrix.copyFrom(input.bodyToEcefCoordinateTransformationMatrix);
1870 }
1871 }
1872
1873 vx = input.vx;
1874 vy = input.vy;
1875 vz = input.vz;
1876
1877 x = input.x;
1878 y = input.y;
1879 z = input.z;
1880
1881 accelerationBiasX = input.accelerationBiasX;
1882 accelerationBiasY = input.accelerationBiasY;
1883 accelerationBiasZ = input.accelerationBiasZ;
1884
1885 gyroBiasX = input.gyroBiasX;
1886 gyroBiasY = input.gyroBiasY;
1887 gyroBiasZ = input.gyroBiasZ;
1888
1889 // copy covariance
1890 if (input.covariance == null) {
1891 covariance = null;
1892 } else {
1893 if (covariance == null) {
1894 covariance = new Matrix(input.covariance);
1895 } else {
1896 covariance.copyFrom(input.covariance);
1897 }
1898 }
1899 }
1900
1901 /**
1902 * Computes and returns hash code for this instance. Hash codes are almost unique
1903 * values that are useful for fast classification and storage of objects in collections.
1904 *
1905 * @return Hash code.
1906 */
1907 @Override
1908 public int hashCode() {
1909 return Objects.hash(bodyToEcefCoordinateTransformationMatrix, vx, vy, vz, x, y, z,
1910 accelerationBiasX, accelerationBiasY, accelerationBiasZ, gyroBiasX, gyroBiasY, gyroBiasZ,
1911 covariance);
1912 }
1913
1914 /**
1915 * Checks if provided object is a INSLooselyCoupledKalmanState having exactly the same
1916 * contents as this instance.
1917 *
1918 * @param obj Object to be compared.
1919 * @return true if both objects are considered to be equal, false otherwise.
1920 */
1921 @Override
1922 public boolean equals(final Object obj) {
1923 if (this == obj) {
1924 return true;
1925 }
1926 if (obj == null || getClass() != obj.getClass()) {
1927 return false;
1928 }
1929 final var other = (INSLooselyCoupledKalmanState) obj;
1930 return equals(other);
1931 }
1932
1933 /**
1934 * Checks if provided instance has exactly the same contents as this instance.
1935 *
1936 * @param other instance to be compared.
1937 * @return true if both instances are considered to be equal, false otherwise.
1938 */
1939 public boolean equals(final INSLooselyCoupledKalmanState other) {
1940 return equals(other, 0.0);
1941 }
1942
1943 /**
1944 * Checks if provided instance has contents similar to this instance up to provided
1945 * threshold value.
1946 *
1947 * @param other instance to be compared.
1948 * @param threshold maximum difference allowed for values.
1949 * @return true if both instances are considered to be equal (up to provided threshold),
1950 * false otherwise.
1951 */
1952 public boolean equals(final INSLooselyCoupledKalmanState other, final double threshold) {
1953 if (other == null) {
1954 return false;
1955 }
1956
1957 return Math.abs(vx - other.vx) <= threshold
1958 && Math.abs(vy - other.vy) <= threshold
1959 && Math.abs(vz - other.vz) <= threshold
1960 && Math.abs(x - other.x) <= threshold
1961 && Math.abs(y - other.y) <= threshold
1962 && Math.abs(z - other.z) <= threshold
1963 && Math.abs(accelerationBiasX - other.accelerationBiasX) <= threshold
1964 && Math.abs(accelerationBiasY - other.accelerationBiasY) <= threshold
1965 && Math.abs(accelerationBiasZ - other.accelerationBiasZ) <= threshold
1966 && Math.abs(gyroBiasX - other.gyroBiasX) <= threshold
1967 && Math.abs(gyroBiasY - other.gyroBiasY) <= threshold
1968 && Math.abs(gyroBiasZ - other.gyroBiasZ) <= threshold
1969 && other.bodyToEcefCoordinateTransformationMatrix != null
1970 && other.bodyToEcefCoordinateTransformationMatrix.equals(bodyToEcefCoordinateTransformationMatrix,
1971 threshold) && other.covariance != null && other.covariance.equals(covariance, threshold);
1972 }
1973
1974 /**
1975 * Makes a copy of this instance.
1976 *
1977 * @return a copy of this instance.
1978 * @throws CloneNotSupportedException if clone fails for some reason.
1979 */
1980 @Override
1981 protected Object clone() throws CloneNotSupportedException {
1982 final var result = (INSLooselyCoupledKalmanState) super.clone();
1983 copyTo(result);
1984 return result;
1985 }
1986
1987 /**
1988 * Fixes current body to ECEF coordinate transformation matrix to ensure it
1989 * remains orthonormal and valid to build a Coordinate transformation or
1990 * a rotation.
1991 *
1992 * @return a fixed rotation matrix.
1993 * @throws AlgebraException if there are numerical instabilities.
1994 */
1995 private Matrix fixRotationMatrix() throws AlgebraException {
1996 final var decomposer = new SingularValueDecomposer(bodyToEcefCoordinateTransformationMatrix);
1997 decomposer.decompose();
1998
1999 // fixed = u * w * v'
2000 final var u = decomposer.getU();
2001 // make sure all singular values are 1
2002 final var w = Matrix.identity(Rotation3D.INHOM_COORDS, Rotation3D.INHOM_COORDS);
2003 final var v = decomposer.getV();
2004
2005 // fixed = u * w * v'
2006 u.multiply(w);
2007 v.transpose();
2008 u.multiply(v);
2009
2010 return u;
2011 }
2012 }