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