1 /*
2 * Copyright (C) 2019 Alberto Irurueta Carro (alberto@irurueta.com)
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16 package com.irurueta.navigation.inertial;
17
18 import com.irurueta.algebra.AlgebraException;
19 import com.irurueta.algebra.Matrix;
20 import com.irurueta.algebra.Utils;
21 import com.irurueta.geometry.Point3D;
22 import com.irurueta.navigation.frames.ECEFPosition;
23 import com.irurueta.navigation.frames.ECEFVelocity;
24 import com.irurueta.navigation.frames.NEDPosition;
25 import com.irurueta.navigation.frames.NEDVelocity;
26 import com.irurueta.navigation.frames.converters.ECEFtoNEDPositionVelocityConverter;
27 import com.irurueta.navigation.geodesic.Constants;
28 import com.irurueta.navigation.gnss.ECEFPositionAndVelocity;
29 import com.irurueta.navigation.inertial.estimators.ECEFGravityEstimator;
30 import com.irurueta.units.Angle;
31 import com.irurueta.units.AngleConverter;
32 import com.irurueta.units.AngleUnit;
33 import com.irurueta.units.Time;
34 import com.irurueta.units.TimeConverter;
35 import com.irurueta.units.TimeUnit;
36
37 /**
38 * Implements one cycle of the loosely coupled INS/GNSS
39 * Kalman filter plus closed-loop correction of all inertial states.
40 * This implementation is based on the equations defined in "Principles of GNSS, Inertial, and Multisensor
41 * Integrated Navigation Systems, Second Edition" and on the companion software available at:
42 * <a href="https://github.com/ymjdz/MATLAB-Codes/blob/master/LC_KF_Epoch.m">
43 * https://github.com/ymjdz/MATLAB-Codes/blob/master/LC_KF_Epoch.m
44 * </a>
45 */
46 public class INSLooselyCoupledKalmanEpochEstimator {
47
48 /**
49 * Earth rotation rate expressed in radians per second (rad/s).
50 */
51 public static final double EARTH_ROTATION_RATE = Constants.EARTH_ROTATION_RATE;
52
53 /**
54 * The equatorial radius of WGS84 ellipsoid (6378137 m) defining Earth's shape.
55 */
56 public static final double EARTH_EQUATORIAL_RADIUS_WGS84 = Constants.EARTH_EQUATORIAL_RADIUS_WGS84;
57
58 /**
59 * Earth eccentricity as defined on the WGS84 ellipsoid.
60 */
61 public static final double EARTH_ECCENTRICITY = Constants.EARTH_ECCENTRICITY;
62
63 /**
64 * Number of components of position + velocity.
65 */
66 private static final int POS_AND_VEL_COMPONENTS = 6;
67
68 /**
69 * Constructor.
70 * Prevents instantiation of helper class.
71 */
72 private INSLooselyCoupledKalmanEpochEstimator() {
73 }
74
75 /**
76 * Estimates the update of Kalman filter state for a single epoch.
77 *
78 * @param userPosition ECEF user position.
79 * @param userVelocity ECEF user velocity.
80 * @param propagationInterval propagation interval expressed in seconds (s).
81 * @param previousState previous Kalman filter state.
82 * @param bodyKinematics body kinematics containing measured specific force
83 * resolved along body frame axes.
84 * @param config Loosely Coupled Kalman filter configuration.
85 * @return new state of Kalman filter.
86 * @throws AlgebraException if there are numerical instabilities.
87 */
88 public static INSLooselyCoupledKalmanState estimate(
89 final ECEFPosition userPosition, final ECEFVelocity userVelocity, final double propagationInterval,
90 final INSLooselyCoupledKalmanState previousState, final BodyKinematics bodyKinematics,
91 final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
92 final var result = new INSLooselyCoupledKalmanState();
93 estimate(userPosition, userVelocity, propagationInterval, previousState, bodyKinematics, config, result);
94 return result;
95 }
96
97 /**
98 * Estimates the update of Kalman filter state for a single epoch.
99 *
100 * @param userPosition ECEF user position.
101 * @param userVelocity ECEF user velocity.
102 * @param propagationInterval propagation interval expressed in seconds (s).
103 * @param previousState previous Kalman filter state.
104 * @param bodyKinematics body kinematics containing measured specific force
105 * resolved along body frame axes.
106 * @param config Loosely Coupled Kalman filter configuration.
107 * @param result instance where new state of Kalman filter will be
108 * stored.
109 * @throws AlgebraException if there are numerical instabilities.
110 */
111 public static void estimate(
112 final ECEFPosition userPosition, final ECEFVelocity userVelocity,
113 final double propagationInterval, final INSLooselyCoupledKalmanState previousState,
114 final BodyKinematics bodyKinematics, final INSLooselyCoupledKalmanConfig config,
115 final INSLooselyCoupledKalmanState result) throws AlgebraException {
116
117 final var fx = bodyKinematics.getFx();
118 final var fy = bodyKinematics.getFy();
119 final var fz = bodyKinematics.getFz();
120
121 estimate(userPosition, userVelocity, propagationInterval, previousState, fx, fy, fz, config, result);
122 }
123
124 /**
125 * Estimates the update of Kalman filter state for a single epoch.
126 *
127 * @param userPosition ECEF user position.
128 * @param userVelocity ECEF user velocity.
129 * @param propagationInterval propagation interval expressed in seconds (s).
130 * @param previousState previous Kalman filter state.
131 * @param bodyKinematics body kinematics containing measured specific force
132 * resolved along body frame axes.
133 * @param previousLatitude previous latitude solution expressed in radians (rad).
134 * @param config Loosely Coupled Kalman filter configuration.
135 * @return new state of Kalman filter.
136 * @throws AlgebraException if there are numerical instabilities.
137 */
138 public static INSLooselyCoupledKalmanState estimate(
139 final ECEFPosition userPosition, final ECEFVelocity userVelocity, final double propagationInterval,
140 final INSLooselyCoupledKalmanState previousState, final BodyKinematics bodyKinematics,
141 final double previousLatitude, final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
142 final var result = new INSLooselyCoupledKalmanState();
143 estimate(userPosition, userVelocity, propagationInterval, previousState, bodyKinematics, previousLatitude,
144 config, result);
145 return result;
146 }
147
148 /**
149 * Estimates the update of Kalman filter state for a single epoch.
150 *
151 * @param userPosition ECEF user position.
152 * @param userVelocity ECEF user velocity.
153 * @param propagationInterval propagation interval expressed in seconds (s).
154 * @param previousState previous Kalman filter state.
155 * @param bodyKinematics body kinematics containing measured specific force
156 * resolved along body frame axes.
157 * @param previousLatitude previous latitude solution expressed in radians (rad).
158 * @param config Loosely Coupled Kalman filter configuration.
159 * @param result instance where new state of Kalman filter will be
160 * stored.
161 * @throws AlgebraException if there are numerical instabilities.
162 */
163 public static void estimate(
164 final ECEFPosition userPosition, final ECEFVelocity userVelocity, final double propagationInterval,
165 final INSLooselyCoupledKalmanState previousState, final BodyKinematics bodyKinematics,
166 final double previousLatitude, final INSLooselyCoupledKalmanConfig config,
167 final INSLooselyCoupledKalmanState result) throws AlgebraException {
168
169 final var fx = bodyKinematics.getFx();
170 final var fy = bodyKinematics.getFy();
171 final var fz = bodyKinematics.getFz();
172
173 estimate(userPosition, userVelocity, propagationInterval, previousState, fx, fy, fz, previousLatitude, config,
174 result);
175 }
176
177 /**
178 * Estimates the update of Kalman filter state for a single epoch.
179 *
180 * @param userPosition ECEF user position.
181 * @param userVelocity ECEF user velocity.
182 * @param propagationInterval propagation interval expressed in seconds (s).
183 * @param previousState previous Kalman filter state.
184 * @param fx measured specific force resolved along body frame
185 * x-axis and expressed in meters per squared
186 * second (m/s^2).
187 * @param fy measured specific force resolved along body frame
188 * y-axis and expressed in meters per squared
189 * second (m/s^2).
190 * @param fz measured specific force resolved along body frame
191 * z-axis and expressed in meters per squared
192 * second (m/s^2).
193 * @param config Loosely Coupled Kalman filter configuration.
194 * @return new state of Kalman filter.
195 * @throws AlgebraException if there are numerical instabilities.
196 */
197 public static INSLooselyCoupledKalmanState estimate(
198 final ECEFPosition userPosition, final ECEFVelocity userVelocity,
199 final double propagationInterval, final INSLooselyCoupledKalmanState previousState,
200 final double fx, final double fy, final double fz, final INSLooselyCoupledKalmanConfig config)
201 throws AlgebraException {
202 final var result = new INSLooselyCoupledKalmanState();
203 estimate(userPosition, userVelocity, propagationInterval, previousState, fx, fy, fz, config, result);
204 return result;
205 }
206
207 /**
208 * Estimates the update of Kalman filter state for a single epoch.
209 *
210 * @param userPosition ECEF user position.
211 * @param userVelocity ECEF user velocity.
212 * @param propagationInterval propagation interval expressed in seconds (s).
213 * @param previousState previous Kalman filter state.
214 * @param fx measured specific force resolved along body frame
215 * x-axis and expressed in meters per squared
216 * second (m/s^2).
217 * @param fy measured specific force resolved along body frame
218 * y-axis and expressed in meters per squared
219 * second (m/s^2).
220 * @param fz measured specific force resolved along body frame
221 * z-axis and expressed in meters per squared
222 * second (m/s^2).
223 * @param config Loosely Coupled Kalman filter configuration.
224 * @param result instance where new state of Kalman filter will be
225 * stored.
226 * @throws AlgebraException if there are numerical instabilities.
227 */
228 public static void estimate(
229 final ECEFPosition userPosition, final ECEFVelocity userVelocity, final double propagationInterval,
230 final INSLooselyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
231 final INSLooselyCoupledKalmanConfig config, final INSLooselyCoupledKalmanState result)
232 throws AlgebraException {
233
234 final var x = userPosition.getX();
235 final var y = userPosition.getY();
236 final var z = userPosition.getZ();
237
238 final var vx = userVelocity.getVx();
239 final var vy = userVelocity.getVy();
240 final var vz = userVelocity.getVz();
241
242 estimate(x, y, z, vx, vy, vz, propagationInterval, previousState, fx, fy, fz, config, result);
243 }
244
245 /**
246 * Estimates the update of Kalman filter state for a single epoch.
247 *
248 * @param userPosition ECEF user position.
249 * @param userVelocity ECEF user velocity.
250 * @param propagationInterval propagation interval expressed in seconds (s).
251 * @param previousState previous Kalman filter state.
252 * @param fx measured specific force resolved along body frame
253 * x-axis and expressed in meters per squared
254 * second (m/s^2).
255 * @param fy measured specific force resolved along body frame
256 * y-axis and expressed in meters per squared
257 * second (m/s^2).
258 * @param fz measured specific force resolved along body frame
259 * z-axis and expressed in meters per squared
260 * second (m/s^2).
261 * @param previousLatitude previous latitude solution expressed in radians (rad).
262 * @param config Loosely Coupled Kalman filter configuration.
263 * @return new state of Kalman filter.
264 * @throws AlgebraException if there are numerical instabilities.
265 */
266 public static INSLooselyCoupledKalmanState estimate(
267 final ECEFPosition userPosition, final ECEFVelocity userVelocity, final double propagationInterval,
268 final INSLooselyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
269 final double previousLatitude, final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
270 final var result = new INSLooselyCoupledKalmanState();
271 estimate(userPosition, userVelocity, propagationInterval, previousState, fx, fy, fz, previousLatitude, config,
272 result);
273 return result;
274 }
275
276 /**
277 * Estimates the update of Kalman filter state for a single epoch.
278 *
279 * @param userPosition GNSS estimated ECEF user position.
280 * @param userVelocity GNSS estimated ECEF user velocity.
281 * @param propagationInterval propagation interval expressed in seconds (s).
282 * @param previousState previous Kalman filter state.
283 * @param fx measured specific force resolved along body frame
284 * x-axis and expressed in meters per squared
285 * second (m/s^2).
286 * @param fy measured specific force resolved along body frame
287 * y-axis and expressed in meters per squared
288 * second (m/s^2).
289 * @param fz measured specific force resolved along body frame
290 * z-axis and expressed in meters per squared
291 * second (m/s^2).
292 * @param previousLatitude previous latitude solution expressed in radians (rad).
293 * @param config Loosely Coupled Kalman filter configuration.
294 * @param result instance where new state of Kalman filter will be
295 * stored.
296 * @throws AlgebraException if there are numerical instabilities.
297 */
298 public static void estimate(
299 final ECEFPosition userPosition, final ECEFVelocity userVelocity, final double propagationInterval,
300 final INSLooselyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
301 final double previousLatitude, final INSLooselyCoupledKalmanConfig config,
302 final INSLooselyCoupledKalmanState result) throws AlgebraException {
303
304 final var x = userPosition.getX();
305 final var y = userPosition.getY();
306 final var z = userPosition.getZ();
307
308 final var vx = userVelocity.getVx();
309 final var vy = userVelocity.getVy();
310 final var vz = userVelocity.getVz();
311
312 estimate(x, y, z, vx, vy, vz, propagationInterval, previousState, fx, fy, fz, previousLatitude, config, result);
313 }
314
315 /**
316 * Estimates the update of Kalman filter state for a single epoch.
317 *
318 * @param x ECEF x coordinate of user position expressed in
319 * meters (m).
320 * @param y ECEF y coordinate of user position expressed in
321 * meters (m).
322 * @param z ECEF z coordinate of user position expressed in
323 * meters (m).
324 * @param vx ECEF x coordinate of user velocity expressed in
325 * meters per second (m/s).
326 * @param vy ECEF y coordinate of user velocity expressed in
327 * meters per second (m/s).
328 * @param vz ECEF z coordinate of user velocity expressed in
329 * meters per second (m/s).
330 * @param propagationInterval propagation interval expressed in seconds (s).
331 * @param previousState previous Kalman filter state.
332 * @param bodyKinematics body kinematics containing measured specific force
333 * resolved along body frame axes.
334 * @param config Loosely Coupled Kalman filter configuration.
335 * @return new state of Kalman filter.
336 * @throws AlgebraException if there are numerical instabilities.
337 */
338 public static INSLooselyCoupledKalmanState estimate(
339 final double x, final double y, final double z, final double vx, final double vy, final double vz,
340 final double propagationInterval, final INSLooselyCoupledKalmanState previousState,
341 final BodyKinematics bodyKinematics, final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
342 final var result = new INSLooselyCoupledKalmanState();
343 estimate(x, y, z, vx, vy, vz, propagationInterval, previousState, bodyKinematics, config, result);
344 return result;
345 }
346
347 /**
348 * Estimates the update of Kalman filter state for a single epoch.
349 *
350 * @param x ECEF x coordinate of user position expressed in
351 * meters (m).
352 * @param y ECEF y coordinate of user position expressed in
353 * meters (m).
354 * @param z ECEF z coordinate of user position expressed in
355 * meters (m).
356 * @param vx ECEF x coordinate of user velocity expressed in
357 * meters per second (m/s).
358 * @param vy ECEF y coordinate of user velocity expressed in
359 * meters per second (m/s).
360 * @param vz ECEF z coordinate of user velocity expressed in
361 * meters per second (m/s).
362 * @param propagationInterval propagation interval expressed in seconds (s).
363 * @param previousState previous Kalman filter state.
364 * @param bodyKinematics body kinematics containing measured specific force
365 * resolved along body frame axes.
366 * @param config Loosely Coupled Kalman filter configuration.
367 * @param result instance where new state of Kalman filter will be
368 * stored.
369 * @throws AlgebraException if there are numerical instabilities.
370 */
371 public static void estimate(
372 final double x, final double y, final double z, final double vx, final double vy, final double vz,
373 final double propagationInterval, final INSLooselyCoupledKalmanState previousState,
374 final BodyKinematics bodyKinematics, final INSLooselyCoupledKalmanConfig config,
375 final INSLooselyCoupledKalmanState result) throws AlgebraException {
376
377 final var fx = bodyKinematics.getFx();
378 final var fy = bodyKinematics.getFy();
379 final var fz = bodyKinematics.getFz();
380
381 estimate(x, y, z, vx, vy, vz, propagationInterval, previousState, fx, fy, fz, config, result);
382 }
383
384 /**
385 * Estimates the update of Kalman filter state for a single epoch.
386 *
387 * @param x ECEF x coordinate of user position expressed in
388 * meters (m).
389 * @param y ECEF y coordinate of user position expressed in
390 * meters (m).
391 * @param z ECEF z coordinate of user position expressed in
392 * meters (m).
393 * @param vx ECEF x coordinate of user velocity expressed in
394 * meters per second (m/s).
395 * @param vy ECEF y coordinate of user velocity expressed in
396 * meters per second (m/s).
397 * @param vz ECEF z coordinate of user velocity expressed in
398 * meters per second (m/s).
399 * @param propagationInterval propagation interval expressed in seconds (s).
400 * @param previousState previous Kalman filter state.
401 * @param bodyKinematics body kinematics containing measured specific force
402 * resolved along body frame axes.
403 * @param previousLatitude previous latitude solution expressed in radians (rad).
404 * @param config Loosely Coupled Kalman filter configuration.
405 * @return new state of Kalman filter.
406 * @throws AlgebraException if there are numerical instabilities.
407 */
408 public static INSLooselyCoupledKalmanState estimate(
409 final double x, final double y, final double z, final double vx, final double vy, final double vz,
410 final double propagationInterval, final INSLooselyCoupledKalmanState previousState,
411 final BodyKinematics bodyKinematics, final double previousLatitude,
412 final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
413 final var result = new INSLooselyCoupledKalmanState();
414 estimate(x, y, z, vx, vy, vz, propagationInterval, previousState, bodyKinematics, previousLatitude, config,
415 result);
416 return result;
417 }
418
419 /**
420 * Estimates the update of Kalman filter state for a single epoch.
421 *
422 * @param x ECEF x coordinate of user position expressed in
423 * meters (m).
424 * @param y ECEF y coordinate of user position expressed in
425 * meters (m).
426 * @param z ECEF z coordinate of user position expressed in
427 * meters (m).
428 * @param vx ECEF x coordinate of user velocity expressed in
429 * meters per second (m/s).
430 * @param vy ECEF y coordinate of user velocity expressed in
431 * meters per second (m/s).
432 * @param vz ECEF z coordinate of user velocity expressed in
433 * meters per second (m/s).
434 * @param propagationInterval propagation interval expressed in seconds (s).
435 * @param previousState previous Kalman filter state.
436 * @param bodyKinematics body kinematics containing measured specific force
437 * resolved along body frame axes.
438 * @param previousLatitude previous latitude solution expressed in radians (rad).
439 * @param config Loosely Coupled Kalman filter configuration.
440 * @param result instance where new state of Kalman filter will be
441 * stored.
442 * @throws AlgebraException if there are numerical instabilities.
443 */
444 public static void estimate(
445 final double x, final double y, final double z, final double vx, final double vy, final double vz,
446 final double propagationInterval, final INSLooselyCoupledKalmanState previousState,
447 final BodyKinematics bodyKinematics, final double previousLatitude,
448 final INSLooselyCoupledKalmanConfig config, final INSLooselyCoupledKalmanState result)
449 throws AlgebraException {
450
451 final var fx = bodyKinematics.getFx();
452 final var fy = bodyKinematics.getFy();
453 final var fz = bodyKinematics.getFz();
454
455 estimate(x, y, z, vx, vy, vz, propagationInterval, previousState, fx, fy, fz, previousLatitude, config, result);
456 }
457
458 /**
459 * Estimates the update of Kalman filter state for a single epoch.
460 *
461 * @param x ECEF x coordinate of user position expressed in
462 * meters (m).
463 * @param y ECEF y coordinate of user position expressed in
464 * meters (m).
465 * @param z ECEF z coordinate of user position expressed in
466 * meters (m).
467 * @param vx ECEF x coordinate of user velocity expressed in
468 * meters per second (m/s).
469 * @param vy ECEF y coordinate of user velocity expressed in
470 * meters per second (m/s).
471 * @param vz ECEF z coordinate of user velocity expressed in
472 * meters per second (m/s).
473 * @param propagationInterval propagation interval expressed in seconds (s).
474 * @param previousState previous Kalman filter state.
475 * @param fx measured specific force resolved along body frame
476 * x-axis and expressed in meters per squared
477 * second (m/s^2).
478 * @param fy measured specific force resolved along body frame
479 * y-axis and expressed in meters per squared
480 * second (m/s^2).
481 * @param fz measured specific force resolved along body frame
482 * z-axis and expressed in meters per squared
483 * second (m/s^2).
484 * @param config Loosely Coupled Kalman filter configuration.
485 * @return new state of Kalman filter.
486 * @throws AlgebraException if there are numerical instabilities.
487 */
488 public static INSLooselyCoupledKalmanState estimate(
489 final double x, final double y, final double z, final double vx, final double vy, final double vz,
490 final double propagationInterval, final INSLooselyCoupledKalmanState previousState,
491 final double fx, double fy, final double fz, final INSLooselyCoupledKalmanConfig config)
492 throws AlgebraException {
493 final var result = new INSLooselyCoupledKalmanState();
494 estimate(x, y, z, vx, vy, vz, propagationInterval, previousState, fx, fy, fz, config, result);
495 return result;
496 }
497
498 /**
499 * Estimates the update of Kalman filter state for a single epoch.
500 *
501 * @param x ECEF x coordinate of user position expressed in
502 * meters (m).
503 * @param y ECEF y coordinate of user position expressed in
504 * meters (m).
505 * @param z ECEF z coordinate of user position expressed in
506 * meters (m).
507 * @param vx ECEF x coordinate of user velocity expressed in
508 * meters per second (m/s).
509 * @param vy ECEF y coordinate of user velocity expressed in
510 * meters per second (m/s).
511 * @param vz ECEF z coordinate of user velocity expressed in
512 * meters per second (m/s).
513 * @param propagationInterval propagation interval expressed in seconds (s).
514 * @param previousState previous Kalman filter state.
515 * @param fx measured specific force resolved along body frame
516 * x-axis and expressed in meters per squared
517 * second (m/s^2).
518 * @param fy measured specific force resolved along body frame
519 * y-axis and expressed in meters per squared
520 * second (m/s^2).
521 * @param fz measured specific force resolved along body frame
522 * z-axis and expressed in meters per squared
523 * second (m/s^2).
524 * @param config Loosely Coupled Kalman filter configuration.
525 * @param result instance where new state of Kalman filter will be
526 * stored.
527 * @throws AlgebraException if there are numerical instabilities.
528 */
529 public static void estimate(
530 final double x, final double y, final double z, final double vx, final double vy, final double vz,
531 final double propagationInterval, final INSLooselyCoupledKalmanState previousState,
532 final double fx, final double fy, final double fz, final INSLooselyCoupledKalmanConfig config,
533 final INSLooselyCoupledKalmanState result) throws AlgebraException {
534
535 final var prevNedPosition = new NEDPosition();
536 final var prevNedVelocity = new NEDVelocity();
537 ECEFtoNEDPositionVelocityConverter.convertECEFtoNED(
538 previousState.getX(), previousState.getY(), previousState.getZ(),
539 previousState.getVx(), previousState.getVy(), previousState.getVz(), prevNedPosition, prevNedVelocity);
540
541 final var previousLatitude = prevNedPosition.getLatitude();
542
543 estimate(x, y, z, vx, vy, vz, propagationInterval, previousState, fx, fy, fz, previousLatitude, config, result);
544 }
545
546 /**
547 * Estimates the update of Kalman filter state for a single epoch.
548 *
549 * @param x ECEF x coordinate of user position expressed in
550 * meters (m).
551 * @param y ECEF y coordinate of user position expressed in
552 * meters (m).
553 * @param z ECEF z coordinate of user position expressed in
554 * meters (m).
555 * @param vx ECEF x coordinate of user velocity expressed in
556 * meters per second (m/s).
557 * @param vy ECEF y coordinate of user velocity expressed in
558 * meters per second (m/s).
559 * @param vz ECEF z coordinate of user velocity expressed in
560 * meters per second (m/s).
561 * @param propagationInterval propagation interval expressed in seconds (s).
562 * @param previousState previous Kalman filter state.
563 * @param fx measured specific force resolved along body frame
564 * x-axis and expressed in meters per squared
565 * second (m/s^2).
566 * @param fy measured specific force resolved along body frame
567 * y-axis and expressed in meters per squared
568 * second (m/s^2).
569 * @param fz measured specific force resolved along body frame
570 * z-axis and expressed in meters per squared
571 * second (m/s^2).
572 * @param previousLatitude previous latitude solution expressed in radians (rad).
573 * @param config Loosely Coupled Kalman filter configuration.
574 * @return new state of Kalman filter.
575 * @throws AlgebraException if there are numerical instabilities.
576 */
577 public static INSLooselyCoupledKalmanState estimate(
578 final double x, final double y, final double z, final double vx, final double vy, final double vz,
579 final double propagationInterval, final INSLooselyCoupledKalmanState previousState,
580 final double fx, final double fy, final double fz, final double previousLatitude,
581 final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
582 final var result = new INSLooselyCoupledKalmanState();
583 estimate(x, y, z, vx, vy, vz, propagationInterval, previousState, fx, fy, fz, previousLatitude, config, result);
584 return result;
585 }
586
587 /**
588 * Estimates the update of Kalman filter state for a single epoch.
589 *
590 * @param x ECEF x coordinate of user position expressed in
591 * meters (m).
592 * @param y ECEF y coordinate of user position expressed in
593 * meters (m).
594 * @param z ECEF z coordinate of user position expressed in
595 * meters (m).
596 * @param vx ECEF x coordinate of user velocity expressed in
597 * meters per second (m/s).
598 * @param vy ECEF y coordinate of user velocity expressed in
599 * meters per second (m/s).
600 * @param vz ECEF z coordinate of user velocity expressed in
601 * meters per second (m/s).
602 * @param propagationInterval propagation interval expressed in seconds (s).
603 * @param previousState previous Kalman filter state.
604 * @param fx measured specific force resolved along body frame
605 * x-axis and expressed in meters per squared
606 * second (m/s^2).
607 * @param fy measured specific force resolved along body frame
608 * y-axis and expressed in meters per squared
609 * second (m/s^2).
610 * @param fz measured specific force resolved along body frame
611 * z-axis and expressed in meters per squared
612 * second (m/s^2).
613 * @param previousLatitude previous latitude solution expressed in radians (rad).
614 * @param config Loosely Coupled Kalman filter configuration.
615 * @param result instance where new state of Kalman filter will be
616 * stored.
617 * @throws AlgebraException if there are numerical instabilities.
618 */
619 public static void estimate(
620 final double x, final double y, final double z, final double vx, final double vy, final double vz,
621 final double propagationInterval, final INSLooselyCoupledKalmanState previousState,
622 final double fx, final double fy, final double fz, final double previousLatitude,
623 final INSLooselyCoupledKalmanConfig config, final INSLooselyCoupledKalmanState result)
624 throws AlgebraException {
625
626 final var omegaIe = Utils.skewMatrix(new double[]{0.0, 0.0, EARTH_ROTATION_RATE});
627
628 // SYSTEM PROPAGATION PHASE
629
630 // 1. Determine transition matrix using (14.50) (first-order approx)
631 final var phiMatrix = Matrix.identity(
632 INSLooselyCoupledKalmanState.NUM_PARAMS, INSLooselyCoupledKalmanState.NUM_PARAMS);
633
634 final var tmp1 = omegaIe.multiplyByScalarAndReturnNew(propagationInterval);
635 final var tmp2 = phiMatrix.getSubmatrix(0, 0, 2, 2);
636 tmp2.subtract(tmp1);
637
638 phiMatrix.setSubmatrix(0, 0, 2, 2, tmp2);
639
640 final var estCbeOld = previousState.getBodyToEcefCoordinateTransformationMatrix();
641 tmp1.copyFrom(estCbeOld);
642 tmp1.multiplyByScalar(propagationInterval);
643
644 phiMatrix.setSubmatrix(0, 12, 2, 14, tmp1);
645 phiMatrix.setSubmatrix(3, 9, 5, 11, tmp1);
646
647 final var measFibb = new Matrix(BodyKinematics.COMPONENTS, 1);
648 measFibb.setElementAtIndex(0, fx);
649 measFibb.setElementAtIndex(1, fy);
650 measFibb.setElementAtIndex(2, fz);
651
652 estCbeOld.multiply(measFibb, tmp1);
653
654 Utils.skewMatrix(tmp1, tmp2);
655 tmp2.multiplyByScalar(-propagationInterval);
656
657 phiMatrix.setSubmatrix(3, 0, 5, 2, tmp2);
658
659 phiMatrix.getSubmatrix(3, 3, 5, 5, tmp1);
660 tmp2.copyFrom(omegaIe);
661 tmp2.multiplyByScalar(2.0 * propagationInterval);
662 tmp1.subtract(tmp2);
663 phiMatrix.setSubmatrix(3, 3, 5, 5, tmp1);
664
665 final var sinPrevLat = Math.sin(previousLatitude);
666 final var cosPrevLat = Math.cos(previousLatitude);
667 final var sinPrevLat2 = sinPrevLat * sinPrevLat;
668 final var cosPrevLat2 = cosPrevLat * cosPrevLat;
669
670 // From (2.137)
671 final var geocentricRadius = EARTH_EQUATORIAL_RADIUS_WGS84
672 / Math.sqrt(1.0 - Math.pow(EARTH_ECCENTRICITY * sinPrevLat, 2.0)) * Math.sqrt(cosPrevLat2
673 + Math.pow(1.0 - EARTH_ECCENTRICITY * EARTH_ECCENTRICITY, 2.0) * sinPrevLat2);
674
675 final var prevX = previousState.getX();
676 final var prevY = previousState.getY();
677 final var prevZ = previousState.getZ();
678 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(prevX, prevY, prevZ);
679
680 final var previousPositionNorm = Math.sqrt(prevX * prevX + prevY * prevY + prevZ * prevZ);
681
682 final var estRebeOld = new Matrix(com.irurueta.navigation.frames.ECEFPosition.COMPONENTS, 1);
683 estRebeOld.setElementAtIndex(0, prevX);
684 estRebeOld.setElementAtIndex(1, prevY);
685 estRebeOld.setElementAtIndex(2, prevZ);
686
687 final var g = gravity.asMatrix();
688 g.multiplyByScalar(-2.0 * propagationInterval / geocentricRadius);
689
690 final var estRebeOldTrans = estRebeOld.transposeAndReturnNew();
691 estRebeOldTrans.multiplyByScalar(1.0 / previousPositionNorm);
692
693 g.multiply(estRebeOldTrans, tmp1);
694
695 phiMatrix.setSubmatrix(3, 6, 5, 8, tmp1);
696
697 for (var i = 0; i < com.irurueta.navigation.frames.ECEFPosition.COMPONENTS; i++) {
698 phiMatrix.setElementAt(6 + i, 3 + i, propagationInterval);
699 }
700
701 // 2. Determine approximate system noise covariance matrix using (14.82)
702 final var qPrimeMatrix = new Matrix(
703 INSLooselyCoupledKalmanState.NUM_PARAMS,
704 INSLooselyCoupledKalmanState.NUM_PARAMS);
705
706 final var gyroNoisePSD = config.getGyroNoisePSD();
707 final var gyroNoiseValue = gyroNoisePSD * propagationInterval;
708 for (var i = 0; i < 3; i++) {
709 qPrimeMatrix.setElementAt(i, i, gyroNoiseValue);
710 }
711
712 final var accelNoisePSD = config.getAccelerometerNoisePSD();
713 final var accelNoiseValue = accelNoisePSD * propagationInterval;
714 for (var i = 3; i < 6; i++) {
715 qPrimeMatrix.setElementAt(i, i, accelNoiseValue);
716 }
717
718 final var accelBiasPSD = config.getAccelerometerBiasPSD();
719 final var accelBiasValue = accelBiasPSD * propagationInterval;
720 for (var i = 9; i < 12; i++) {
721 qPrimeMatrix.setElementAt(i, i, accelBiasValue);
722 }
723
724 final var gyroBiasPSD = config.getGyroBiasPSD();
725 final var gyroBiasValue = gyroBiasPSD * propagationInterval;
726 for (var i = 12; i < 15; i++) {
727 qPrimeMatrix.setElementAt(i, i, gyroBiasValue);
728 }
729
730 // 3. Propagate state estimates using (3.14) noting that all states are zero
731 // due to closed-loop correction.
732 // x_est_propagated(1:15, 1) = 0
733
734 // 4. Propagate state estimation error covariance matrix using (3.46)
735 final var pMatrixOld = previousState.getCovariance();
736
737 qPrimeMatrix.multiplyByScalar(0.5);
738
739 final var tmp3 = pMatrixOld.addAndReturnNew(qPrimeMatrix);
740 final var pMatrixPropagated = phiMatrix.multiplyAndReturnNew(tmp3);
741
742 phiMatrix.transpose();
743 pMatrixPropagated.multiply(phiMatrix);
744
745 pMatrixPropagated.add(qPrimeMatrix);
746
747 // MEASUREMENT UPDATE PHASE
748
749 // 5. Set-up measurement matrix using (14.115)
750 final var h = new Matrix(POS_AND_VEL_COMPONENTS, INSLooselyCoupledKalmanState.NUM_PARAMS);
751 for (var i = 0; i < 3; i++) {
752 h.setElementAt(i, POS_AND_VEL_COMPONENTS + i, -1.0);
753 final var j = ECEFPosition.COMPONENTS + i;
754 h.setElementAt(j, j, -1.0);
755 }
756
757 // 6. Set-up measurement noise covariance matrix assuming all components of
758 // GNSS position and velocity are independent and have equal variance.
759 final var r = new Matrix(POS_AND_VEL_COMPONENTS, POS_AND_VEL_COMPONENTS);
760
761 final var posMeasSD = config.getPositionNoiseSD();
762 final var posMeasSD2 = posMeasSD * posMeasSD;
763 for (var i = 0; i < com.irurueta.navigation.frames.ECEFPosition.COMPONENTS; i++) {
764 r.setElementAt(i, i, posMeasSD2);
765 }
766
767 final var velMeasSD = config.getVelocityNoiseSD();
768 final var velMeasSD2 = velMeasSD * velMeasSD;
769 for (var i = com.irurueta.navigation.frames.ECEFPosition.COMPONENTS; i < POS_AND_VEL_COMPONENTS; i++) {
770 r.setElementAt(i, i, velMeasSD2);
771 }
772
773 // 7. Calculate Kalman gain using (3.21)
774 final var hTrans = h.transposeAndReturnNew();
775
776 final var tmp4 = h.multiplyAndReturnNew(pMatrixPropagated);
777 tmp4.multiply(hTrans);
778 tmp4.add(r);
779
780 final var tmp5 = Utils.inverse(tmp4);
781
782 final var k = pMatrixPropagated.multiplyAndReturnNew(hTrans);
783 k.multiply(tmp5);
784
785 // 8. Formulate measurement innovations using (14.102), noting that zero
786 // lever arm is assumed here
787 final var prevVx = previousState.getVx();
788 final var prevVy = previousState.getVy();
789 final var prevVz = previousState.getVz();
790
791 final var deltaZ = new Matrix(POS_AND_VEL_COMPONENTS, 1);
792 deltaZ.setElementAtIndex(0, x - prevX);
793 deltaZ.setElementAtIndex(1, y - prevY);
794 deltaZ.setElementAtIndex(2, z - prevZ);
795 deltaZ.setElementAtIndex(3, vx - prevVx);
796 deltaZ.setElementAtIndex(4, vy - prevVy);
797 deltaZ.setElementAtIndex(5, vz - prevVz);
798
799 // 9. Update state estimates using (3.24)
800 // x_est_new = x_est_propagated + K_matrix * delta_z
801 final var xEstNew = k.multiplyAndReturnNew(deltaZ);
802
803 // 10. Update state estimation error covariance matrix using (3.25)
804 k.multiply(h);
805 final var pNew = Matrix.identity(INSLooselyCoupledKalmanState.NUM_PARAMS,
806 INSLooselyCoupledKalmanState.NUM_PARAMS);
807 pNew.subtract(k);
808 pNew.multiply(pMatrixPropagated);
809
810 // CLOSED-LOOP CORRECTION
811
812 // Correct attitude, velocity, and position using (14.7-9)
813 final var tmp6 = xEstNew.getSubmatrix(0, 0, 2, 0);
814 final var tmp7 = Utils.skewMatrix(tmp6);
815
816 final var estCbeNew = Matrix.identity(ECEFPosition.COMPONENTS, ECEFPosition.COMPONENTS);
817 estCbeNew.subtract(tmp7);
818 estCbeNew.multiply(estCbeOld);
819
820 final var newVx = prevVx - xEstNew.getElementAtIndex(3);
821 final var newVy = prevVy - xEstNew.getElementAtIndex(4);
822 final var newVz = prevVz - xEstNew.getElementAtIndex(5);
823
824 final var newX = prevX - xEstNew.getElementAtIndex(6);
825 final var newY = prevY - xEstNew.getElementAtIndex(7);
826 final var newZ = prevZ - xEstNew.getElementAtIndex(8);
827
828 // Update IMU bias estimates
829 final var newAccelerationBiasX = previousState.getAccelerationBiasX() + xEstNew.getElementAtIndex(9);
830 final var newAccelerationBiasY = previousState.getAccelerationBiasY() + xEstNew.getElementAtIndex(10);
831 final var newAccelerationBiasZ = previousState.getAccelerationBiasZ() + xEstNew.getElementAtIndex(11);
832
833 final var newGyroBiasX = previousState.getGyroBiasX() + xEstNew.getElementAtIndex(12);
834 final var newGyroBiasY = previousState.getGyroBiasY() + xEstNew.getElementAtIndex(13);
835 final var newGyroBiasZ = previousState.getGyroBiasZ() + xEstNew.getElementAtIndex(14);
836
837 // set result values
838 result.setBodyToEcefCoordinateTransformationMatrix(estCbeNew);
839 result.setVelocityCoordinates(newVx, newVy, newVz);
840 result.setPositionCoordinates(newX, newY, newZ);
841 result.setAccelerationBiasCoordinates(newAccelerationBiasX, newAccelerationBiasY, newAccelerationBiasZ);
842 result.setGyroBiasCoordinates(newGyroBiasX, newGyroBiasY, newGyroBiasZ);
843 result.setCovariance(pNew);
844 }
845
846 /**
847 * Estimates the update of Kalman filter state for a single epoch.
848 *
849 * @param userPosition ECEF user position.
850 * @param userVelocity ECEF user velocity.
851 * @param propagationInterval propagation interval expressed in seconds (s).
852 * @param previousState previous Kalman filter state.
853 * @param bodyKinematics body kinematics containing measured specific force
854 * resolved along body frame axes.
855 * @param config Loosely Coupled Kalman filter configuration.
856 * @return new state of Kalman filter.
857 * @throws AlgebraException if there are numerical instabilities.
858 */
859 public static INSLooselyCoupledKalmanState estimate(
860 final ECEFPosition userPosition, final ECEFVelocity userVelocity, final Time propagationInterval,
861 final INSLooselyCoupledKalmanState previousState, final BodyKinematics bodyKinematics,
862 final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
863 return estimate(userPosition, userVelocity, convertTime(propagationInterval), previousState, bodyKinematics,
864 config);
865 }
866
867 /**
868 * Estimates the update of Kalman filter state for a single epoch.
869 *
870 * @param userPosition ECEF user position.
871 * @param userVelocity ECEF user velocity.
872 * @param propagationInterval propagation interval.
873 * @param previousState previous Kalman filter state.
874 * @param bodyKinematics body kinematics containing measured specific force
875 * resolved along body frame axes.
876 * @param config Loosely Coupled Kalman filter configuration.
877 * @param result instance where new state of Kalman filter will be
878 * stored.
879 * @throws AlgebraException if there are numerical instabilities.
880 */
881 public static void estimate(
882 final ECEFPosition userPosition, final ECEFVelocity userVelocity, final Time propagationInterval,
883 final INSLooselyCoupledKalmanState previousState, final BodyKinematics bodyKinematics,
884 final INSLooselyCoupledKalmanConfig config, final INSLooselyCoupledKalmanState result)
885 throws AlgebraException {
886 estimate(userPosition, userVelocity, convertTime(propagationInterval), previousState, bodyKinematics, config,
887 result);
888 }
889
890 /**
891 * Estimates the update of Kalman filter state for a single epoch.
892 *
893 * @param userPosition ECEF user position.
894 * @param userVelocity ECEF user velocity.
895 * @param propagationInterval propagation interval.
896 * @param previousState previous Kalman filter state.
897 * @param bodyKinematics body kinematics containing measured specific force
898 * resolved along body frame axes.
899 * @param previousLatitude previous latitude solution expressed in radians (rad).
900 * @param config Loosely Coupled Kalman filter configuration.
901 * @return new state of Kalman filter.
902 * @throws AlgebraException if there are numerical instabilities.
903 */
904 public static INSLooselyCoupledKalmanState estimate(
905 final ECEFPosition userPosition, final ECEFVelocity userVelocity, final Time propagationInterval,
906 final INSLooselyCoupledKalmanState previousState, final BodyKinematics bodyKinematics,
907 final double previousLatitude, final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
908 return estimate(userPosition, userVelocity, convertTime(propagationInterval), previousState, bodyKinematics,
909 previousLatitude, config);
910 }
911
912 /**
913 * Estimates the update of Kalman filter state of a single epoch.
914 *
915 * @param userPosition ECEF user position.
916 * @param userVelocity ECEF user velocity.
917 * @param propagationInterval propagation interval.
918 * @param previousState previous Kalman filter state.
919 * @param bodyKinematics body kinematics containing measured specific force
920 * resolved along body frame axes.
921 * @param previousLatitude previous latitude solution expressed in radians (rad).
922 * @param config Loosely Coupled Kalman filter configuration.
923 * @param result instance where new state of Kalman filter will be
924 * stored.
925 * @throws AlgebraException if there are numerical instabilities.
926 */
927 public static void estimate(
928 final ECEFPosition userPosition, final ECEFVelocity userVelocity, final Time propagationInterval,
929 final INSLooselyCoupledKalmanState previousState, final BodyKinematics bodyKinematics,
930 final double previousLatitude, final INSLooselyCoupledKalmanConfig config,
931 final INSLooselyCoupledKalmanState result) throws AlgebraException {
932 estimate(userPosition, userVelocity, convertTime(propagationInterval), previousState, bodyKinematics,
933 previousLatitude, config, result);
934 }
935
936 /**
937 * Estimates the update of Kalman filter state of a single epoch.
938 *
939 * @param userPosition ECEF user position.
940 * @param userVelocity ECEF user velocity.
941 * @param propagationInterval propagation interval.
942 * @param previousState previous Kalman filter state.
943 * @param fx measured specific force resolved along body frame
944 * x-axis and expressed in meters per squared
945 * second (m/s^2).
946 * @param fy measured specific force resolved along body frame
947 * y-axis and expressed in meters per squared
948 * second (m/s^2).
949 * @param fz measured specific force resolved along body frame
950 * z-axis and expressed in meters per squared
951 * second (m/s^2).
952 * @param config Loosely Coupled Kalman filter configuration.
953 * @return new state of Kalman filter.
954 * @throws AlgebraException if there are numerical instabilities.
955 */
956 public static INSLooselyCoupledKalmanState estimate(
957 final ECEFPosition userPosition, final ECEFVelocity userVelocity, final Time propagationInterval,
958 final INSLooselyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
959 final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
960 return estimate(userPosition, userVelocity, convertTime(propagationInterval), previousState, fx, fy, fz,
961 config);
962 }
963
964 /**
965 * Estimates the update of Kalman filter state of a single epoch.
966 *
967 * @param userPosition ECEF user position.
968 * @param userVelocity ECEF user velocity.
969 * @param propagationInterval propagation interval.
970 * @param previousState previous Kalman filter state.
971 * @param fx measured specific force resolved along body frame
972 * x-axis and expressed in meters per squared
973 * second (m/s^2).
974 * @param fy measured specific force resolved along body frame
975 * y-axis and expressed in meters per squared
976 * second (m/s^2).
977 * @param fz measured specific force resolved along body frame
978 * z-axis and expressed in meters per squared
979 * second (m/s^2).
980 * @param config Loosely Coupled Kalman filter configuration.
981 * @param result instance where new state of Kalman filter will be
982 * stored.
983 * @throws AlgebraException if there are numerical instabilities.
984 */
985 public static void estimate(
986 final ECEFPosition userPosition, final ECEFVelocity userVelocity, final Time propagationInterval,
987 final INSLooselyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
988 final INSLooselyCoupledKalmanConfig config, final INSLooselyCoupledKalmanState result)
989 throws AlgebraException {
990 estimate(userPosition, userVelocity, convertTime(propagationInterval), previousState, fx, fy, fz, config,
991 result);
992 }
993
994 /**
995 * Estimates the update of Kalman filter state of a single epoch.
996 *
997 * @param userPosition ECEF user position.
998 * @param userVelocity ECEF user velocity.
999 * @param propagationInterval propagation interval.
1000 * @param previousState previous Kalman filter state.
1001 * @param fx measured specific force resolved along body frame
1002 * x-axis and expressed in meters per squared
1003 * second (m/s^2).
1004 * @param fy measured specific force resolved along body frame
1005 * y-axis and expressed in meters per squared
1006 * second (m/s^2).
1007 * @param fz measured specific force resolved along body frame
1008 * z-axis and expressed in meters per squared
1009 * second (m/s^2).
1010 * @param previousLatitude previous latitude solution expressed in radians (rad).
1011 * @param config Loosely Coupled Kalman filter configuration.
1012 * @return new state of Kalman filter.
1013 * @throws AlgebraException if there are numerical instabilities.
1014 */
1015 public static INSLooselyCoupledKalmanState estimate(
1016 final ECEFPosition userPosition, final ECEFVelocity userVelocity, final Time propagationInterval,
1017 final INSLooselyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
1018 final double previousLatitude, final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
1019 return estimate(userPosition, userVelocity, convertTime(propagationInterval), previousState, fx, fy, fz,
1020 previousLatitude, config);
1021 }
1022
1023 /**
1024 * Estimates the update of Kalman filter state of a single epoch.
1025 *
1026 * @param userPosition ECEF user position.
1027 * @param userVelocity ECEF user velocity.
1028 * @param propagationInterval propagation interval.
1029 * @param previousState previous Kalman filter state.
1030 * @param fx measured specific force resolved along body frame
1031 * x-axis and expressed in meters per squared
1032 * second (m/s^2).
1033 * @param fy measured specific force resolved along body frame
1034 * y-axis and expressed in meters per squared
1035 * second (m/s^2).
1036 * @param fz measured specific force resolved along body frame
1037 * z-axis and expressed in meters per squared
1038 * second (m/s^2).
1039 * @param previousLatitude previous latitude solution expressed in radians (rad).
1040 * @param config Loosely Coupled Kalman filter configuration.
1041 * @param result instance where new state of Kalman filter will be
1042 * stored.
1043 * @throws AlgebraException if there are numerical instabilities.
1044 */
1045 public static void estimate(
1046 final ECEFPosition userPosition, final ECEFVelocity userVelocity, final Time propagationInterval,
1047 final INSLooselyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
1048 final double previousLatitude, final INSLooselyCoupledKalmanConfig config,
1049 final INSLooselyCoupledKalmanState result) throws AlgebraException {
1050 estimate(userPosition, userVelocity, convertTime(propagationInterval), previousState, fx, fy, fz,
1051 previousLatitude, config, result);
1052 }
1053
1054 /**
1055 * Estimates the update of Kalman filter state of a single epoch.
1056 *
1057 * @param x ECEF x coordinate of user position expressed in
1058 * meters (m).
1059 * @param y ECEF y coordinate of user position expressed in
1060 * meters (m).
1061 * @param z ECEF z coordinate of user position expressed in
1062 * meters (m).
1063 * @param vx ECEF x coordinate of user velocity expressed in
1064 * meters per second (m/s).
1065 * @param vy ECEF y coordinate of user velocity expressed in
1066 * meters per second (m/s).
1067 * @param vz ECEF z coordinate of user velocity expressed in
1068 * meters per second (m/s).
1069 * @param propagationInterval propagation interval.
1070 * @param previousState previous Kalman filter state.
1071 * @param bodyKinematics body kinematics containing measured specific force
1072 * resolved along body frame axes.
1073 * @param config Loosely Coupled Kalman filter configuration.
1074 * @return new state of Kalman filter.
1075 * @throws AlgebraException if there are numerical instabilities.
1076 */
1077 public static INSLooselyCoupledKalmanState estimate(
1078 final double x, final double y, final double z, final double vx, final double vy, final double vz,
1079 final Time propagationInterval, final INSLooselyCoupledKalmanState previousState,
1080 final BodyKinematics bodyKinematics, final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
1081 return estimate(x, y, z, vx, vy, vz, convertTime(propagationInterval), previousState, bodyKinematics, config);
1082 }
1083
1084 /**
1085 * Estimates the update of Kalman filter state for a single epoch.
1086 *
1087 * @param x ECEF x coordinate of user position expressed in
1088 * meters (m).
1089 * @param y ECEF y coordinate of user position expressed in
1090 * meters (m).
1091 * @param z ECEF z coordinate of user position expressed in
1092 * meters (m).
1093 * @param vx ECEF x coordinate of user velocity expressed in
1094 * meters per second (m/s).
1095 * @param vy ECEF y coordinate of user velocity expressed in
1096 * meters per second (m/s).
1097 * @param vz ECEF z coordinate of user velocity expressed in
1098 * meters per second (m/s).
1099 * @param propagationInterval propagation interval.
1100 * @param previousState previous Kalman filter state.
1101 * @param bodyKinematics body kinematics containing measured specific force
1102 * resolved along body frame axes.
1103 * @param config Loosely Coupled Kalman filter configuration.
1104 * @param result instance where new state of Kalman filter will be
1105 * stored.
1106 * @throws AlgebraException if there are numerical instabilities.
1107 */
1108 public static void estimate(
1109 final double x, final double y, final double z, final double vx, final double vy, final double vz,
1110 final Time propagationInterval, final INSLooselyCoupledKalmanState previousState,
1111 final BodyKinematics bodyKinematics, final INSLooselyCoupledKalmanConfig config,
1112 final INSLooselyCoupledKalmanState result) throws AlgebraException {
1113 estimate(x, y, z, vx, vy, vz, convertTime(propagationInterval), previousState, bodyKinematics, config, result);
1114 }
1115
1116 /**
1117 * Estimates the update of Kalman filter state for a single epoch.
1118 *
1119 * @param x ECEF x coordinate of user position expressed in
1120 * meters (m).
1121 * @param y ECEF y coordinate of user position expressed in
1122 * meters (m).
1123 * @param z ECEF z coordinate of user position expressed in
1124 * meters (m).
1125 * @param vx ECEF x coordinate of user velocity expressed in
1126 * meters per second (m/s).
1127 * @param vy ECEF y coordinate of user velocity expressed in
1128 * meters per second (m/s).
1129 * @param vz ECEF z coordinate of user velocity expressed in
1130 * meters per second (m/s).
1131 * @param propagationInterval propagation interval.
1132 * @param previousState previous Kalman filter state.
1133 * @param bodyKinematics body kinematics containing measured specific force
1134 * resolved along body frame axes.
1135 * @param previousLatitude previous latitude solution expressed in radians (rad).
1136 * @param config Loosely Coupled Kalman filter configuration.
1137 * @return new state of Kalman filter.
1138 * @throws AlgebraException if there are numerical instabilities.
1139 */
1140 public static INSLooselyCoupledKalmanState estimate(
1141 final double x, final double y, final double z, final double vx, final double vy, final double vz,
1142 final Time propagationInterval, final INSLooselyCoupledKalmanState previousState,
1143 final BodyKinematics bodyKinematics, final double previousLatitude,
1144 final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
1145 return estimate(x, y, z, vx, vy, vz, convertTime(propagationInterval), previousState, bodyKinematics,
1146 previousLatitude, config);
1147 }
1148
1149 /**
1150 * Estimates the update of Kalman filter state for a single epoch.
1151 *
1152 * @param x ECEF x coordinate of user position expressed in
1153 * meters (m).
1154 * @param y ECEF y coordinate of user position expressed in
1155 * meters (m).
1156 * @param z ECEF z coordinate of user position expressed in
1157 * meters (m).
1158 * @param vx ECEF x coordinate of user velocity expressed in
1159 * meters per second (m/s).
1160 * @param vy ECEF y coordinate of user velocity expressed in
1161 * meters per second (m/s).
1162 * @param vz ECEF z coordinate of user velocity expressed in
1163 * meters per second (m/s).
1164 * @param propagationInterval propagation interval.
1165 * @param previousState previous Kalman filter state.
1166 * @param bodyKinematics body kinematics containing measured specific force
1167 * resolved along body frame axes.
1168 * @param previousLatitude previous latitude solution expressed in radians (rad).
1169 * @param config Loosely Coupled Kalman filter configuration.
1170 * @param result instance where new state of Kalman filter will be
1171 * stored.
1172 * @throws AlgebraException if there are numerical instabilities.
1173 */
1174 public static void estimate(
1175 final double x, final double y, final double z, final double vx, final double vy, final double vz,
1176 final Time propagationInterval, final INSLooselyCoupledKalmanState previousState,
1177 final BodyKinematics bodyKinematics, final double previousLatitude,
1178 final INSLooselyCoupledKalmanConfig config, final INSLooselyCoupledKalmanState result)
1179 throws AlgebraException {
1180 estimate(x, y, z, vx, vy, vz, convertTime(propagationInterval), previousState, bodyKinematics, previousLatitude,
1181 config, result);
1182 }
1183
1184 /**
1185 * Estimates the update of Kalman filter state for a single epoch.
1186 *
1187 * @param x ECEF x coordinate of user position expressed in
1188 * meters (m).
1189 * @param y ECEF y coordinate of user position expressed in
1190 * meters (m).
1191 * @param z ECEF z coordinate of user position expressed in
1192 * meters (m).
1193 * @param vx ECEF x coordinate of user velocity expressed in
1194 * meters per second (m/s).
1195 * @param vy ECEF y coordinate of user velocity expressed in
1196 * meters per second (m/s).
1197 * @param vz ECEF z coordinate of user velocity expressed in
1198 * meters per second (m/s).
1199 * @param propagationInterval propagation interval.
1200 * @param previousState previous Kalman filter state.
1201 * @param fx measured specific force resolved along body frame
1202 * x-axis and expressed in meters per squared
1203 * second (m/s^2).
1204 * @param fy measured specific force resolved along body frame
1205 * y-axis and expressed in meters per squared
1206 * second (m/s^2).
1207 * @param fz measured specific force resolved along body frame
1208 * z-axis and expressed in meters per squared
1209 * second (m/s^2).
1210 * @param config Loosely Coupled Kalman filter configuration.
1211 * @return new state of Kalman filter.
1212 * @throws AlgebraException if there are numerical instabilities.
1213 */
1214 public static INSLooselyCoupledKalmanState estimate(
1215 final double x, final double y, final double z, final double vx, final double vy, final double vz,
1216 final Time propagationInterval, final INSLooselyCoupledKalmanState previousState,
1217 final double fx, final double fy, final double fz, final INSLooselyCoupledKalmanConfig config)
1218 throws AlgebraException {
1219 return estimate(x, y, z, vx, vy, vz, convertTime(propagationInterval), previousState, fx, fy, fz, config);
1220 }
1221
1222 /**
1223 * Estimated the update of Kalman filter state for a single epoch.
1224 *
1225 * @param x ECEF x coordinate of user position expressed in
1226 * meters (m).
1227 * @param y ECEF y coordinate of user position expressed in
1228 * meters (m).
1229 * @param z ECEF z coordinate of user position expressed in
1230 * meters (m).
1231 * @param vx ECEF x coordinate of user velocity expressed in
1232 * meters per second (m/s).
1233 * @param vy ECEF y coordinate of user velocity expressed in
1234 * meters per second (m/s).
1235 * @param vz ECEF z coordinate of user velocity expressed in
1236 * meters per second (m/s).
1237 * @param propagationInterval propagation interval.
1238 * @param previousState previous Kalman filter state.
1239 * @param fx measured specific force resolved along body frame
1240 * x-axis and expressed in meters per squared
1241 * second (m/s^2).
1242 * @param fy measured specific force resolved along body frame
1243 * y-axis and expressed in meters per squared
1244 * second (m/s^2).
1245 * @param fz measured specific force resolved along body frame
1246 * z-axis and expressed in meters per squared
1247 * second (m/s^2).
1248 * @param config Loosely Coupled Kalman filter configuration.
1249 * @param result instance where new state of Kalman filter will be
1250 * stored.
1251 * @throws AlgebraException if there are numerical instabilities.
1252 */
1253 public static void estimate(
1254 final double x, final double y, final double z, final double vx, final double vy, final double vz,
1255 final Time propagationInterval, final INSLooselyCoupledKalmanState previousState,
1256 final double fx, final double fy, final double fz, final INSLooselyCoupledKalmanConfig config,
1257 final INSLooselyCoupledKalmanState result) throws AlgebraException {
1258 estimate(x, y, z, vx, vy, vz, convertTime(propagationInterval), previousState, fx, fy, fz, config, result);
1259 }
1260
1261 /**
1262 * Estimates the update of Kalman filter state for a single epoch.
1263 *
1264 * @param x ECEF x coordinate of user position expressed in
1265 * meters (m).
1266 * @param y ECEF y coordinate of user position expressed in
1267 * meters (m).
1268 * @param z ECEF z coordinate of user position expressed in
1269 * meters (m).
1270 * @param vx ECEF x coordinate of user velocity expressed in
1271 * meters per second (m/s).
1272 * @param vy ECEF y coordinate of user velocity expressed in
1273 * meters per second (m/s).
1274 * @param vz ECEF z coordinate of user velocity expressed in
1275 * meters per second (m/s).
1276 * @param propagationInterval propagation interval.
1277 * @param previousState previous Kalman filter state.
1278 * @param fx measured specific force resolved along body frame
1279 * x-axis and expressed in meters per squared
1280 * second (m/s^2).
1281 * @param fy measured specific force resolved along body frame
1282 * y-axis and expressed in meters per squared
1283 * second (m/s^2).
1284 * @param fz measured specific force resolved along body frame
1285 * z-axis and expressed in meters per squared
1286 * second (m/s^2).
1287 * @param previousLatitude previous latitude solution expressed in radians (rad).
1288 * @param config Loosely Coupled Kalman filter configuration.
1289 * @return new state of Kalman filter.
1290 * @throws AlgebraException if there are numerical instabilities.
1291 */
1292 public static INSLooselyCoupledKalmanState estimate(
1293 final double x, final double y, final double z, final double vx, final double vy, final double vz,
1294 final Time propagationInterval, final INSLooselyCoupledKalmanState previousState,
1295 final double fx, final double fy, final double fz, final double previousLatitude,
1296 final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
1297 return estimate(x, y, z, vx, vy, vz, convertTime(propagationInterval), previousState, fx, fy, fz,
1298 previousLatitude, config);
1299 }
1300
1301 /**
1302 * Estimates the update of Kalman filter state for a single epoch.
1303 *
1304 * @param x ECEF x coordinate of user position expressed in
1305 * meters (m).
1306 * @param y ECEF y coordinate of user position expressed in
1307 * meters (m).
1308 * @param z ECEF z coordinate of user position expressed in
1309 * meters (m).
1310 * @param vx ECEF x coordinate of user velocity expressed in
1311 * meters per second (m/s).
1312 * @param vy ECEF y coordinate of user velocity expressed in
1313 * meters per second (m/s).
1314 * @param vz ECEF z coordinate of user velocity expressed in
1315 * meters per second (m/s).
1316 * @param propagationInterval propagation interval.
1317 * @param previousState previous Kalman filter state.
1318 * @param fx measured specific force resolved along body frame
1319 * x-axis and expressed in meters per squared
1320 * second (m/s^2).
1321 * @param fy measured specific force resolved along body frame
1322 * y-axis and expressed in meters per squared
1323 * second (m/s^2).
1324 * @param fz measured specific force resolved along body frame
1325 * z-axis and expressed in meters per squared
1326 * second (m/s^2).
1327 * @param previousLatitude previous latitude solution expressed in radians (rad).
1328 * @param config Loosely Coupled Kalman filter configuration.
1329 * @param result instance where new state of Kalman filter will be
1330 * stored.
1331 * @throws AlgebraException if there are numerical instabilities.
1332 */
1333 public static void estimate(
1334 final double x, final double y, final double z, final double vx, final double vy, final double vz,
1335 final Time propagationInterval, final INSLooselyCoupledKalmanState previousState,
1336 final double fx, final double fy, final double fz, final double previousLatitude,
1337 final INSLooselyCoupledKalmanConfig config, final INSLooselyCoupledKalmanState result)
1338 throws AlgebraException {
1339 estimate(x, y, z, vx, vy, vz, convertTime(propagationInterval), previousState, fx, fy, fz, previousLatitude,
1340 config, result);
1341 }
1342
1343 /**
1344 * Estimates the update of Kalman filter state for a single epoch.
1345 *
1346 * @param userPosition ECEF user position expressed in meters (m).
1347 * @param userVelocity ECEf user velocity.
1348 * @param propagationInterval propagation interval expressed in seconds (s).
1349 * @param previousState previous Kalman filter state.
1350 * @param bodyKinematics body kinematics containing measured specific force
1351 * resolved along body frame axes.
1352 * @param config Loosely Coupled Kalman filter configuration.
1353 * @return new state of Kalman filter.
1354 * @throws AlgebraException if there are numerical instabilities.
1355 */
1356 public static INSLooselyCoupledKalmanState estimate(
1357 final Point3D userPosition, final ECEFVelocity userVelocity,
1358 final double propagationInterval, final INSLooselyCoupledKalmanState previousState,
1359 final BodyKinematics bodyKinematics, final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
1360 return estimate(userPosition.getInhomX(), userPosition.getInhomY(), userPosition.getInhomZ(),
1361 userVelocity.getVx(), userVelocity.getVy(), userVelocity.getVz(), propagationInterval, previousState,
1362 bodyKinematics, config);
1363 }
1364
1365 /**
1366 * Estimates the update of Kalman filter state for a single epoch.
1367 *
1368 * @param userPosition ECEF user position expressed in meters (m).
1369 * @param userVelocity ECEF user velocity.
1370 * @param propagationInterval propagation interval expressed in seconds (s).
1371 * @param previousState previous Kalman filter state.
1372 * @param bodyKinematics body kinematics containing measured specific force
1373 * resolved along body frame axes.
1374 * @param config Loosely Coupled Kalman filter configuration.
1375 * @param result instance where new state of Kalman filter will be
1376 * stored.
1377 * @throws AlgebraException if there are numerical instabilities.
1378 */
1379 public static void estimate(
1380 final Point3D userPosition, final ECEFVelocity userVelocity, final double propagationInterval,
1381 final INSLooselyCoupledKalmanState previousState, final BodyKinematics bodyKinematics,
1382 final INSLooselyCoupledKalmanConfig config, final INSLooselyCoupledKalmanState result)
1383 throws AlgebraException {
1384 estimate(userPosition.getInhomX(), userPosition.getInhomY(), userPosition.getInhomZ(),
1385 userVelocity.getVx(), userVelocity.getVy(), userVelocity.getVz(), propagationInterval, previousState,
1386 bodyKinematics, config, result);
1387 }
1388
1389 /**
1390 * Estimates the update of Kalman filter state for a single epoch.
1391 *
1392 * @param userPosition ECEF user position expressed in meters (m).
1393 * @param userVelocity ECEF user velocity.
1394 * @param propagationInterval propagation interval expressed in seconds (s).
1395 * @param previousState previous Kalman filter state.
1396 * @param bodyKinematics body kinematics containing measured specific force
1397 * resolved along body frame axes.
1398 * @param previousLatitude previous latitude solution expressed in radians (rad).
1399 * @param config Loosely Coupled Kalman filter configuration.
1400 * @return new state of Kalman filter.
1401 * @throws AlgebraException if there are numerical instabilities.
1402 */
1403 public static INSLooselyCoupledKalmanState estimate(
1404 final Point3D userPosition, final ECEFVelocity userVelocity, final double propagationInterval,
1405 final INSLooselyCoupledKalmanState previousState, final BodyKinematics bodyKinematics,
1406 final double previousLatitude, final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
1407 return estimate(userPosition.getInhomX(), userPosition.getInhomY(), userPosition.getInhomZ(),
1408 userVelocity.getVx(), userVelocity.getVy(), userVelocity.getVz(), propagationInterval,
1409 previousState, bodyKinematics, previousLatitude, config);
1410 }
1411
1412 /**
1413 * Estimates the update of Kalman filter state for a single epoch.
1414 *
1415 * @param userPosition ECEF user position expressed in meters (m).
1416 * @param userVelocity ECEF user velocity.
1417 * @param propagationInterval propagation interval expressed in seconds (s).
1418 * @param previousState previous Kalman filter state.
1419 * @param bodyKinematics body kinematics containing measured specific force
1420 * resolved along body frame axes.
1421 * @param previousLatitude previous latitude solution expressed in radians (rad).
1422 * @param config Loosely Coupled Kalman filter configuration.
1423 * @param result instance where new state of Kalman filter will be
1424 * stored.
1425 * @throws AlgebraException if there are numerical instabilities.
1426 */
1427 public static void estimate(
1428 final Point3D userPosition, final ECEFVelocity userVelocity, final double propagationInterval,
1429 final INSLooselyCoupledKalmanState previousState, final BodyKinematics bodyKinematics,
1430 final double previousLatitude, final INSLooselyCoupledKalmanConfig config,
1431 final INSLooselyCoupledKalmanState result) throws AlgebraException {
1432 estimate(userPosition.getInhomX(), userPosition.getInhomY(), userPosition.getInhomZ(),
1433 userVelocity.getVx(), userVelocity.getVy(), userVelocity.getVz(), propagationInterval, previousState,
1434 bodyKinematics, previousLatitude, config, result);
1435 }
1436
1437 /**
1438 * Estimates the update of Kalman filter state for a single epoch.
1439 *
1440 * @param userPosition ECEF user position expressed in meters (m).
1441 * @param userVelocity ECEF user velocity.
1442 * @param propagationInterval propagation interval expressed in seconds (s).
1443 * @param previousState previous Kalman filter state.
1444 * @param fx measured specific force resolved along body frame
1445 * x-axis and expressed in meters per squared
1446 * second (m/s^2).
1447 * @param fy measured specific force resolved along body frame
1448 * y-axis and expressed in meters per squared
1449 * second (m/s^2).
1450 * @param fz measured specific force resolved along body frame
1451 * z-axis and expressed in meters per squared
1452 * second (m/s^2).
1453 * @param config Loosely Coupled Kalman filter configuration.
1454 * @return new state of Kalman filter.
1455 * @throws AlgebraException if there are numerical instabilities.
1456 */
1457 public static INSLooselyCoupledKalmanState estimate(
1458 final Point3D userPosition, final ECEFVelocity userVelocity, final double propagationInterval,
1459 final INSLooselyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
1460 final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
1461 return estimate(userPosition.getInhomX(), userPosition.getInhomY(), userPosition.getInhomZ(),
1462 userVelocity.getVx(), userVelocity.getVy(), userVelocity.getVz(), propagationInterval, previousState,
1463 fx, fy, fz, config);
1464 }
1465
1466 /**
1467 * Estimates the update of Kalman filter state for a single epoch.
1468 *
1469 * @param userPosition ECEF user position expressed in meters (m).
1470 * @param userVelocity ECEF user velocity.
1471 * @param propagationInterval propagation interval expressed in seconds (s).
1472 * @param previousState previous Kalman filter state.
1473 * @param fx measured specific force resolved along body frame
1474 * x-axis and expressed in meters per squared
1475 * second (m/s^2).
1476 * @param fy measured specific force resolved along body frame
1477 * y-axis and expressed in meters per squared
1478 * second (m/s^2).
1479 * @param fz measured specific force resolved along body frame
1480 * z-axis and expressed in meters per squared
1481 * second (m/s^2).
1482 * @param config Loosely Coupled Kalman filter configuration.
1483 * @param result instance where new state of Kalman filter will be
1484 * stored.
1485 * @throws AlgebraException if there are numerical instabilities.
1486 */
1487 public static void estimate(
1488 final Point3D userPosition, final ECEFVelocity userVelocity,
1489 final double propagationInterval, final INSLooselyCoupledKalmanState previousState,
1490 final double fx, final double fy, final double fz, final INSLooselyCoupledKalmanConfig config,
1491 final INSLooselyCoupledKalmanState result) throws AlgebraException {
1492 estimate(userPosition.getInhomX(), userPosition.getInhomY(), userPosition.getInhomZ(),
1493 userVelocity.getVx(), userVelocity.getVy(), userVelocity.getVz(), propagationInterval, previousState,
1494 fx, fy, fz, config, result);
1495 }
1496
1497 /**
1498 * Estimates the update of Kalman filter state for a single epoch.
1499 *
1500 * @param userPosition ECEF user position expressed in meters (m).
1501 * @param userVelocity ECEF user velocity.
1502 * @param propagationInterval propagation interval expressed in seconds (s).
1503 * @param previousState previous Kalman filter state.
1504 * @param fx measured specific force resolved along body frame
1505 * x-axis and expressed in meters per squared
1506 * second (m/s^2).
1507 * @param fy measured specific force resolved along body frame
1508 * y-axis and expressed in meters per squared
1509 * second (m/s^2).
1510 * @param fz measured specific force resolved along body frame
1511 * z-axis and expressed in meters per squared
1512 * second (m/s^2).
1513 * @param previousLatitude previous latitude solution expressed in radians (rad).
1514 * @param config Loosely Coupled Kalman filter configuration.
1515 * @return new state of Kalman filter.
1516 * @throws AlgebraException if there are numerical instabilities.
1517 */
1518 public static INSLooselyCoupledKalmanState estimate(
1519 final Point3D userPosition, final ECEFVelocity userVelocity, final double propagationInterval,
1520 final INSLooselyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
1521 final double previousLatitude, final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
1522 return estimate(userPosition.getInhomX(), userPosition.getInhomY(), userPosition.getInhomZ(),
1523 userVelocity.getVx(), userVelocity.getVy(), userVelocity.getVz(), propagationInterval, previousState,
1524 fx, fy, fz, previousLatitude, config);
1525 }
1526
1527 /**
1528 * Estimates the update of Kalman filter state for a single epoch.
1529 *
1530 * @param userPosition ECEF user position expressed in meters (m).
1531 * @param userVelocity ECEF user velocity.
1532 * @param propagationInterval propagation interval expressed in seconds (s).
1533 * @param previousState previous Kalman filter state.
1534 * @param fx measured specific force resolved along body frame
1535 * x-axis and expressed in meters per squared
1536 * second (m/s^2).
1537 * @param fy measured specific force resolved along body frame
1538 * y-axis and expressed in meters per squared
1539 * second (m/s^2).
1540 * @param fz measured specific force resolved along body frame
1541 * z-axis and expressed in meters per squared
1542 * second (m/s^2).
1543 * @param previousLatitude previous latitude solution expressed in radians (rad).
1544 * @param config Loosely Coupled Kalman filter configuration.
1545 * @param result instance where new state of Kalman filter will be
1546 * stored.
1547 * @throws AlgebraException if there are numerical instabilities.
1548 */
1549 public static void estimate(
1550 final Point3D userPosition, final ECEFVelocity userVelocity, final double propagationInterval,
1551 final INSLooselyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
1552 final double previousLatitude, final INSLooselyCoupledKalmanConfig config,
1553 final INSLooselyCoupledKalmanState result) throws AlgebraException {
1554 estimate(userPosition.getInhomX(), userPosition.getInhomY(), userPosition.getInhomZ(),
1555 userVelocity.getVx(), userVelocity.getVy(), userVelocity.getVz(), propagationInterval, previousState,
1556 fx, fy, fz, previousLatitude, config, result);
1557 }
1558
1559 /**
1560 * Estimates the update of Kalman filter state for a single epoch.
1561 *
1562 * @param userPosition ECEF user position expressed in meters (m).
1563 * @param userVelocity ECEF user velocity.
1564 * @param propagationInterval propagation interval.
1565 * @param previousState previous Kalman filter state.
1566 * @param bodyKinematics body kinematics containing measured specific force
1567 * resolved along body frame axes.
1568 * @param config Loosely Coupled Kalman filter configuration.
1569 * @return new state of Kalman filter.
1570 * @throws AlgebraException if there are numerical instabilities.
1571 */
1572 public static INSLooselyCoupledKalmanState estimate(
1573 final Point3D userPosition, final ECEFVelocity userVelocity, final Time propagationInterval,
1574 final INSLooselyCoupledKalmanState previousState, final BodyKinematics bodyKinematics,
1575 final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
1576 return estimate(userPosition.getInhomX(), userPosition.getInhomY(), userPosition.getInhomZ(),
1577 userVelocity.getVx(), userVelocity.getVy(), userVelocity.getVz(), propagationInterval, previousState,
1578 bodyKinematics, config);
1579 }
1580
1581 /**
1582 * Estimates the update of Kalman filter state for a single epoch.
1583 *
1584 * @param userPosition ECEF user position expressed in meters (m).
1585 * @param userVelocity ECEF user velocity.
1586 * @param propagationInterval propagation interval.
1587 * @param previousState previous Kalman filter state.
1588 * @param bodyKinematics body kinematics containing measured specific force
1589 * resolved along body frame axes.
1590 * @param config Loosely Coupled Kalman filter configuration.
1591 * @param result instance where new state of Kalman filter will be
1592 * stored.
1593 * @throws AlgebraException if there are numerical instabilities.
1594 */
1595 public static void estimate(
1596 final Point3D userPosition, final ECEFVelocity userVelocity,
1597 final Time propagationInterval, final INSLooselyCoupledKalmanState previousState,
1598 final BodyKinematics bodyKinematics, final INSLooselyCoupledKalmanConfig config,
1599 final INSLooselyCoupledKalmanState result) throws AlgebraException {
1600 estimate(userPosition.getInhomX(), userPosition.getInhomY(), userPosition.getInhomZ(),
1601 userVelocity.getVx(), userVelocity.getVy(), userVelocity.getVz(), propagationInterval, previousState,
1602 bodyKinematics, config, result);
1603 }
1604
1605 /**
1606 * Estimates the update of Kalman filter state for a single epoch.
1607 *
1608 * @param userPosition ECEF user position expressed in meters (m).
1609 * @param userVelocity ECEF user velocity.
1610 * @param propagationInterval propagation interval.
1611 * @param previousState previous Kalman filter state.
1612 * @param bodyKinematics body kinematics containing measured specific force
1613 * resolved along body frame axes.
1614 * @param previousLatitude previous latitude solution expressed in radians (rad).
1615 * @param config Loosely Coupled Kalman filter configuration.
1616 * @return new state of Kalman filter.
1617 * @throws AlgebraException if there are numerical instabilities.
1618 */
1619 public static INSLooselyCoupledKalmanState estimate(
1620 final Point3D userPosition, final ECEFVelocity userVelocity,
1621 final Time propagationInterval, final INSLooselyCoupledKalmanState previousState,
1622 final BodyKinematics bodyKinematics, final double previousLatitude,
1623 final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
1624 return estimate(userPosition.getInhomX(), userPosition.getInhomY(), userPosition.getInhomZ(),
1625 userVelocity.getVx(), userVelocity.getVy(), userVelocity.getVz(), propagationInterval, previousState,
1626 bodyKinematics, previousLatitude, config);
1627 }
1628
1629 /**
1630 * Estimates the update of Kalman filter state for a single epoch.
1631 *
1632 * @param userPosition ECEF user position expressed in meters (m).
1633 * @param userVelocity ECEF user velocity.
1634 * @param propagationInterval propagation interval.
1635 * @param previousState previous Kalman filter state.
1636 * @param bodyKinematics body kinematics containing measured specific force
1637 * resolved along body frame axes.
1638 * @param previousLatitude previous latitude solution expressed in radians (rad).
1639 * @param config Loosely Coupled Kalman filter configuration.
1640 * @param result instance where new state of Kalman filter will be
1641 * stored.
1642 * @throws AlgebraException if there are numerical instabilities.
1643 */
1644 public static void estimate(
1645 final Point3D userPosition, final ECEFVelocity userVelocity, final Time propagationInterval,
1646 final INSLooselyCoupledKalmanState previousState, final BodyKinematics bodyKinematics,
1647 final double previousLatitude, final INSLooselyCoupledKalmanConfig config,
1648 final INSLooselyCoupledKalmanState result) throws AlgebraException {
1649 estimate(userPosition.getInhomX(), userPosition.getInhomY(), userPosition.getInhomZ(),
1650 userVelocity.getVx(), userVelocity.getVy(), userVelocity.getVz(), propagationInterval, previousState,
1651 bodyKinematics, previousLatitude, config, result);
1652 }
1653
1654 /**
1655 * Estimates the update of Kalman filter state for a single epoch.
1656 *
1657 * @param userPosition ECEF user position expressed in meters (m).
1658 * @param userVelocity ECEF user velocity.
1659 * @param propagationInterval propagation interval.
1660 * @param previousState previous Kalman filter state.
1661 * @param fx measured specific force resolved along body frame
1662 * x-axis and expressed in meters per squared
1663 * second (m/s^2).
1664 * @param fy measured specific force resolved along body frame
1665 * y-axis and expressed in meters per squared
1666 * second (m/s^2).
1667 * @param fz measured specific force resolved along body frame
1668 * z-axis and expressed in meters per squared
1669 * second (m/s^2).
1670 * @param config Loosely Coupled Kalman filter configuration.
1671 * @return new state of Kalman filter.
1672 * @throws AlgebraException if there are numerical instabilities.
1673 */
1674 public static INSLooselyCoupledKalmanState estimate(
1675 final Point3D userPosition, final ECEFVelocity userVelocity, final Time propagationInterval,
1676 final INSLooselyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
1677 final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
1678 return estimate(userPosition.getInhomX(), userPosition.getInhomY(), userPosition.getInhomZ(),
1679 userVelocity.getVx(), userVelocity.getVy(), userVelocity.getVz(), propagationInterval, previousState,
1680 fx, fy, fz, config);
1681 }
1682
1683 /**
1684 * Estimates the update of Kalman filter state for a single epoch.
1685 *
1686 * @param userPosition ECEF user position expressed in meters (m).
1687 * @param userVelocity ECEF user velocity.
1688 * @param propagationInterval propagation interval.
1689 * @param previousState previous Kalman filter state.
1690 * @param fx measured specific force resolved along body frame
1691 * x-axis and expressed in meters per squared
1692 * second (m/s^2).
1693 * @param fy measured specific force resolved along body frame
1694 * y-axis and expressed in meters per squared
1695 * second (m/s^2).
1696 * @param fz measured specific force resolved along body frame
1697 * z-axis and expressed in meters per squared
1698 * second (m/s^2).
1699 * @param config Loosely Coupled Kalman filter configuration.
1700 * @param result instance where new state of Kalman filter will be
1701 * stored.
1702 * @throws AlgebraException if there are numerical instabilities.
1703 */
1704 public static void estimate(
1705 final Point3D userPosition, final ECEFVelocity userVelocity, final Time propagationInterval,
1706 final INSLooselyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
1707 final INSLooselyCoupledKalmanConfig config, final INSLooselyCoupledKalmanState result)
1708 throws AlgebraException {
1709 estimate(userPosition.getInhomX(), userPosition.getInhomY(), userPosition.getInhomZ(),
1710 userVelocity.getVx(), userVelocity.getVy(), userVelocity.getVz(), propagationInterval, previousState,
1711 fx, fy, fz, config, result);
1712 }
1713
1714 /**
1715 * Estimates the update of Kalman filter state for a single epoch.
1716 *
1717 * @param userPosition ECEF user position expressed in meters (m).
1718 * @param userVelocity ECEF user velocity.
1719 * @param propagationInterval propagation interval.
1720 * @param previousState previous Kalman filter state.
1721 * @param fx measured specific force resolved along body frame
1722 * x-axis and expressed in meters per squared
1723 * second (m/s^2).
1724 * @param fy measured specific force resolved along body frame
1725 * y-axis and expressed in meters per squared
1726 * second (m/s^2).
1727 * @param fz measured specific force resolved along body frame
1728 * z-axis and expressed in meters per squared
1729 * second (m/s^2).
1730 * @param previousLatitude previous latitude solution expressed in radians (rad).
1731 * @param config Loosely Coupled Kalman filter configuration.
1732 * @return new state of Kalman filter.
1733 * @throws AlgebraException if there are numerical instabilities.
1734 */
1735 public static INSLooselyCoupledKalmanState estimate(
1736 final Point3D userPosition, final ECEFVelocity userVelocity, final Time propagationInterval,
1737 final INSLooselyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
1738 final double previousLatitude, final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
1739 return estimate(userPosition.getInhomX(), userPosition.getInhomY(), userPosition.getInhomZ(),
1740 userVelocity.getVx(), userVelocity.getVy(), userVelocity.getVz(), propagationInterval, previousState,
1741 fx, fy, fz, previousLatitude, config);
1742 }
1743
1744 /**
1745 * Estimates the update of Kalman filter state for a single epoch.
1746 *
1747 * @param userPosition ECEF user position expressed in meters (m).
1748 * @param userVelocity ECEF user velocity.
1749 * @param propagationInterval propagation interval.
1750 * @param previousState previous Kalman filter state.
1751 * @param fx measured specific force resolved along body frame
1752 * x-axis and expressed in meters per squared
1753 * second (m/s^2).
1754 * @param fy measured specific force resolved along body frame
1755 * y-axis and expressed in meters per squared
1756 * second (m/s^2).
1757 * @param fz measured specific force resolved along body frame
1758 * z-axis and expressed in meters per squared
1759 * second (m/s^2).
1760 * @param previousLatitude previous latitude solution expressed in radians (rad).
1761 * @param config Loosely Coupled Kalman filter configuration.
1762 * @param result instance where new state of Kalman filter will be
1763 * stored.
1764 * @throws AlgebraException if there are numerical instabilities.
1765 */
1766 public static void estimate(
1767 final Point3D userPosition, final ECEFVelocity userVelocity, final Time propagationInterval,
1768 final INSLooselyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
1769 final double previousLatitude, final INSLooselyCoupledKalmanConfig config,
1770 final INSLooselyCoupledKalmanState result) throws AlgebraException {
1771 estimate(userPosition.getInhomX(), userPosition.getInhomY(), userPosition.getInhomZ(),
1772 userVelocity.getVx(), userVelocity.getVy(), userVelocity.getVz(), propagationInterval, previousState,
1773 fx, fy, fz, previousLatitude, config, result);
1774 }
1775
1776 /**
1777 * Estimates the update of Kalman filter state for a single epoch.
1778 *
1779 * @param positionAndVelocity ECEF user position and velocity.
1780 * @param propagationInterval propagation interval expressed in seconds (s).
1781 * @param previousState previous Kalman filter state.
1782 * @param bodyKinematics body kinematics containing measured specific force
1783 * resolved along body frame axes.
1784 * @param config Loosely Coupled Kalman filter configuration.
1785 * @return new state of Kalman filter.
1786 * @throws AlgebraException if there are numerical instabilities.
1787 */
1788 public static INSLooselyCoupledKalmanState estimate(
1789 final ECEFPositionAndVelocity positionAndVelocity, final double propagationInterval,
1790 final INSLooselyCoupledKalmanState previousState, final BodyKinematics bodyKinematics,
1791 final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
1792 return estimate(positionAndVelocity.getX(), positionAndVelocity.getY(), positionAndVelocity.getZ(),
1793 positionAndVelocity.getVx(), positionAndVelocity.getVy(), positionAndVelocity.getVz(),
1794 propagationInterval, previousState, bodyKinematics, config);
1795 }
1796
1797 /**
1798 * Estimates the update of Kalman filter state for a single epoch.
1799 *
1800 * @param positionAndVelocity ECEF user position and velocity.
1801 * @param propagationInterval propagation interval expressed in seconds (s).
1802 * @param previousState previous Kalman filter state.
1803 * @param bodyKinematics body kinematics containing measured specific force
1804 * resolved along body frame axes.
1805 * @param config Loosely Coupled Kalman filter configuration.
1806 * @param result instance where new state of Kalman filter will be
1807 * stored.
1808 * @throws AlgebraException if there are numerical instabilities.
1809 */
1810 public static void estimate(
1811 final ECEFPositionAndVelocity positionAndVelocity, final double propagationInterval,
1812 final INSLooselyCoupledKalmanState previousState, final BodyKinematics bodyKinematics,
1813 final INSLooselyCoupledKalmanConfig config, final INSLooselyCoupledKalmanState result)
1814 throws AlgebraException {
1815 estimate(positionAndVelocity.getX(), positionAndVelocity.getY(), positionAndVelocity.getZ(),
1816 positionAndVelocity.getVx(), positionAndVelocity.getVy(), positionAndVelocity.getVz(),
1817 propagationInterval, previousState, bodyKinematics, config, result);
1818 }
1819
1820 /**
1821 * Estimates the update of Kalman filter state for a single epoch.
1822 *
1823 * @param positionAndVelocity ECEF user position and velocity.
1824 * @param propagationInterval propagation interval expressed in seconds (s).
1825 * @param previousState previous Kalman filter state.
1826 * @param bodyKinematics body kinematics containing measured specific force
1827 * resolved along body frame axes.
1828 * @param previousLatitude previous latitude solution expressed in radians (rad).
1829 * @param config Loosely Coupled Kalman filter configuration.
1830 * @return new state of Kalman filter.
1831 * @throws AlgebraException if there are numerical instabilities.
1832 */
1833 public static INSLooselyCoupledKalmanState estimate(
1834 final ECEFPositionAndVelocity positionAndVelocity, final double propagationInterval,
1835 final INSLooselyCoupledKalmanState previousState, final BodyKinematics bodyKinematics,
1836 final double previousLatitude, final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
1837 return estimate(positionAndVelocity.getX(), positionAndVelocity.getY(), positionAndVelocity.getZ(),
1838 positionAndVelocity.getVx(), positionAndVelocity.getVy(), positionAndVelocity.getVz(),
1839 propagationInterval, previousState, bodyKinematics, previousLatitude, config);
1840 }
1841
1842 /**
1843 * Estimates the update of Kalman filter state for a single epoch.
1844 *
1845 * @param positionAndVelocity ECEF user position and velocity.
1846 * @param propagationInterval propagation interval expressed in seconds (s).
1847 * @param previousState previous Kalman filter state.
1848 * @param bodyKinematics body kinematics containing measured specific force
1849 * resolved along body frame axes.
1850 * @param previousLatitude previous latitude solution expressed in radians (rad).
1851 * @param config Loosely Coupled Kalman filter configuration.
1852 * @param result instance where new state of Kalman filter will be
1853 * stored.
1854 * @throws AlgebraException if there are numerical instabilities.
1855 */
1856 public static void estimate(
1857 final ECEFPositionAndVelocity positionAndVelocity, final double propagationInterval,
1858 final INSLooselyCoupledKalmanState previousState, final BodyKinematics bodyKinematics,
1859 final double previousLatitude, final INSLooselyCoupledKalmanConfig config,
1860 final INSLooselyCoupledKalmanState result) throws AlgebraException {
1861 estimate(positionAndVelocity.getX(), positionAndVelocity.getY(), positionAndVelocity.getZ(),
1862 positionAndVelocity.getVx(), positionAndVelocity.getVy(), positionAndVelocity.getVz(),
1863 propagationInterval, previousState, bodyKinematics, previousLatitude, config, result);
1864 }
1865
1866 /**
1867 * Estimates the update of Kalman filter state for a single epoch.
1868 *
1869 * @param positionAndVelocity ECEF user position and velocity.
1870 * @param propagationInterval propagation interval expressed in seconds (s).
1871 * @param previousState previous Kalman filter state.
1872 * @param fx measured specific force resolved along body frame
1873 * x-axis and expressed in meters per squared
1874 * second (m/s^2).
1875 * @param fy measured specific force resolved along body frame
1876 * y-axis and expressed in meters per squared
1877 * second (m/s^2).
1878 * @param fz measured specific force resolved along body frame
1879 * z-axis and expressed in meters per squared
1880 * second (m/s^2).
1881 * @param config Loosely Coupled Kalman filter configuration.
1882 * @return new state of Kalman filter.
1883 * @throws AlgebraException if there are numerical instabilities.
1884 */
1885 public static INSLooselyCoupledKalmanState estimate(
1886 final ECEFPositionAndVelocity positionAndVelocity, final double propagationInterval,
1887 final INSLooselyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
1888 final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
1889 return estimate(positionAndVelocity.getX(), positionAndVelocity.getY(), positionAndVelocity.getZ(),
1890 positionAndVelocity.getVx(), positionAndVelocity.getVy(), positionAndVelocity.getVz(),
1891 propagationInterval, previousState, fx, fy, fz, config);
1892 }
1893
1894 /**
1895 * Estimates the update of Kalman filter state for a single epoch.
1896 *
1897 * @param positionAndVelocity ECEF user position and velocity.
1898 * @param propagationInterval propagation interval expressed in seconds (s).
1899 * @param previousState previous Kalman filter state.
1900 * @param fx measured specific force resolved along body frame
1901 * x-axis and expressed in meters per squared
1902 * second (m/s^2).
1903 * @param fy measured specific force resolved along body frame
1904 * y-axis and expressed in meters per squared
1905 * second (m/s^2).
1906 * @param fz measured specific force resolved along body frame
1907 * z-axis and expressed in meters per squared
1908 * second (m/s^2).
1909 * @param config Loosely Coupled Kalman filter configuration.
1910 * @param result instance where new state of Kalman filter will be stored.
1911 * @throws AlgebraException if there are numerical instabilities.
1912 */
1913 public static void estimate(
1914 final ECEFPositionAndVelocity positionAndVelocity, final double propagationInterval,
1915 final INSLooselyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
1916 final INSLooselyCoupledKalmanConfig config, final INSLooselyCoupledKalmanState result)
1917 throws AlgebraException {
1918 estimate(positionAndVelocity.getX(), positionAndVelocity.getY(), positionAndVelocity.getZ(),
1919 positionAndVelocity.getVx(), positionAndVelocity.getVy(), positionAndVelocity.getVz(),
1920 propagationInterval, previousState, fx, fy, fz, config, result);
1921 }
1922
1923 /**
1924 * Estimates the update of Kalman filter state for a single epoch.
1925 *
1926 * @param positionAndVelocity ECEF user position and velocity.
1927 * @param propagationInterval propagation interval expressed in seconds (s).
1928 * @param previousState previous Kalman filter state.
1929 * @param fx measured specific force resolved along body frame
1930 * x-axis and expressed in meters per squared
1931 * second (m/s^2).
1932 * @param fy measured specific force resolved along body frame
1933 * y-axis and expressed in meters per squared
1934 * second (m/s^2).
1935 * @param fz measured specific force resolved along body frame
1936 * z-axis and expressed in meters per squared
1937 * second (m/s^2).
1938 * @param previousLatitude previous latitude solution expressed in radians (rad).
1939 * @param config Loosely Coupled Kalman filter configuration.
1940 * @return new state of Kalman filter.
1941 * @throws AlgebraException if there are numerical instabilities.
1942 */
1943 public static INSLooselyCoupledKalmanState estimate(
1944 final ECEFPositionAndVelocity positionAndVelocity, final double propagationInterval,
1945 final INSLooselyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
1946 final double previousLatitude, final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
1947 return estimate(positionAndVelocity.getX(), positionAndVelocity.getY(), positionAndVelocity.getZ(),
1948 positionAndVelocity.getVx(), positionAndVelocity.getVy(), positionAndVelocity.getVz(),
1949 propagationInterval, previousState, fx, fy, fz, previousLatitude, config);
1950 }
1951
1952 /**
1953 * Estimates the update of Kalman filter state for a single epoch.
1954 *
1955 * @param positionAndVelocity ECEF user position and velocity.
1956 * @param propagationInterval propagation interval expressed in seconds (s).
1957 * @param previousState previous Kalman filter state.
1958 * @param fx measured specific force resolved along body frame
1959 * x-axis and expressed in meters per squared
1960 * second (m/s^2).
1961 * @param fy measured specific force resolved along body frame
1962 * y-axis and expressed in meters per squared
1963 * second (m/s^2).
1964 * @param fz measured specific force resolved along body frame
1965 * z-axis and expressed in meters per squared
1966 * second (m/s^2).
1967 * @param previousLatitude previous latitude solution expressed in radians (rad).
1968 * @param config Loosely Coupled Kalman filter configuration.
1969 * @param result instance where new state of Kalman filter will be
1970 * stored.
1971 * @throws AlgebraException if there are numerical instabilities.
1972 */
1973 public static void estimate(
1974 final ECEFPositionAndVelocity positionAndVelocity, final double propagationInterval,
1975 final INSLooselyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
1976 final double previousLatitude, final INSLooselyCoupledKalmanConfig config,
1977 final INSLooselyCoupledKalmanState result) throws AlgebraException {
1978 estimate(positionAndVelocity.getX(), positionAndVelocity.getY(), positionAndVelocity.getZ(),
1979 positionAndVelocity.getVx(), positionAndVelocity.getVy(), positionAndVelocity.getVz(),
1980 propagationInterval, previousState, fx, fy, fz, previousLatitude, config, result);
1981 }
1982
1983 /**
1984 * Estimates the update of Kalman filter state for a single epoch.
1985 *
1986 * @param positionAndVelocity ECEF user position and velocity.
1987 * @param propagationInterval propagation interval.
1988 * @param previousState previous Kalman filter state.
1989 * @param bodyKinematics body kinematics containing measured specific force
1990 * resolved along body frame axes.
1991 * @param config Loosely Coupled Kalman filter configuration.
1992 * @return new state of Kalman filter.
1993 * @throws AlgebraException if there are numerical instabilities.
1994 */
1995 public static INSLooselyCoupledKalmanState estimate(
1996 final ECEFPositionAndVelocity positionAndVelocity, final Time propagationInterval,
1997 final INSLooselyCoupledKalmanState previousState, final BodyKinematics bodyKinematics,
1998 final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
1999 return estimate(positionAndVelocity.getX(), positionAndVelocity.getY(), positionAndVelocity.getZ(),
2000 positionAndVelocity.getVx(), positionAndVelocity.getVy(), positionAndVelocity.getVz(),
2001 propagationInterval, previousState, bodyKinematics, config);
2002 }
2003
2004 /**
2005 * Estimates the update of Kalman filter state for a single epoch.
2006 *
2007 * @param positionAndVelocity ECEF user position and velocity.
2008 * @param propagationInterval propagation interval.
2009 * @param previousState previous Kalman filter state.
2010 * @param bodyKinematics body kinematics containing measured specific force
2011 * resolved along body frame axes.
2012 * @param config Loosely Coupled Kalman filter configuration.
2013 * @param result instance where new state of Kalman filter will be
2014 * stored.
2015 * @throws AlgebraException if there are numerical instabilities.
2016 */
2017 public static void estimate(
2018 final ECEFPositionAndVelocity positionAndVelocity, final Time propagationInterval,
2019 final INSLooselyCoupledKalmanState previousState, final BodyKinematics bodyKinematics,
2020 final INSLooselyCoupledKalmanConfig config, final INSLooselyCoupledKalmanState result)
2021 throws AlgebraException {
2022 estimate(positionAndVelocity.getX(), positionAndVelocity.getY(), positionAndVelocity.getZ(),
2023 positionAndVelocity.getVx(), positionAndVelocity.getVy(), positionAndVelocity.getVz(),
2024 propagationInterval, previousState, bodyKinematics, config, result);
2025 }
2026
2027 /**
2028 * Estimates the update of Kalman filter state for a single epoch.
2029 *
2030 * @param positionAndVelocity ECEF user position and velocity.
2031 * @param propagationInterval propagation interval.
2032 * @param previousState previous Kalman filter state.
2033 * @param bodyKinematics body kinematics containing measured specific force
2034 * resolved along body frame axes.
2035 * @param previousLatitude previous latitude solution expressed in radians (rad).
2036 * @param config Loosely Coupled Kalman filter configuration.
2037 * @return new state of Kalman filter.
2038 * @throws AlgebraException if there are numerical instabilities.
2039 */
2040 public static INSLooselyCoupledKalmanState estimate(
2041 final ECEFPositionAndVelocity positionAndVelocity, final Time propagationInterval,
2042 final INSLooselyCoupledKalmanState previousState, final BodyKinematics bodyKinematics,
2043 final double previousLatitude, final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
2044 return estimate(positionAndVelocity.getX(), positionAndVelocity.getY(), positionAndVelocity.getZ(),
2045 positionAndVelocity.getVx(), positionAndVelocity.getVy(), positionAndVelocity.getVz(),
2046 propagationInterval, previousState, bodyKinematics, previousLatitude, config);
2047 }
2048
2049 /**
2050 * Estimates the update of Kalman filter state for a single epoch.
2051 *
2052 * @param positionAndVelocity ECEF user position and velocity.
2053 * @param propagationInterval propagation interval.
2054 * @param previousState previous Kalman filter state.
2055 * @param bodyKinematics body kinematics containing measured specific force
2056 * resolved along body frame axes.
2057 * @param previousLatitude previous latitude solution expressed in radians (rad).
2058 * @param config Loosely Coupled Kalman filter configuration.
2059 * @param result instance where new state of Kalman filter will be
2060 * stored.
2061 * @throws AlgebraException if there are numerical instabilities.
2062 */
2063 public static void estimate(
2064 final ECEFPositionAndVelocity positionAndVelocity, final Time propagationInterval,
2065 final INSLooselyCoupledKalmanState previousState, final BodyKinematics bodyKinematics,
2066 final double previousLatitude, final INSLooselyCoupledKalmanConfig config,
2067 final INSLooselyCoupledKalmanState result) throws AlgebraException {
2068 estimate(positionAndVelocity.getX(), positionAndVelocity.getY(), positionAndVelocity.getZ(),
2069 positionAndVelocity.getVx(), positionAndVelocity.getVy(), positionAndVelocity.getVz(),
2070 propagationInterval, previousState, bodyKinematics, previousLatitude, config, result);
2071 }
2072
2073 /**
2074 * Estimates the update of Kalman filter state for a single epoch.
2075 *
2076 * @param positionAndVelocity ECEF user position and velocity.
2077 * @param propagationInterval propagation interval.
2078 * @param previousState previous Kalman filter state.
2079 * @param fx measured specific force resolved along body frame
2080 * x-axis and expressed in meters per squared
2081 * second (m/s^2).
2082 * @param fy measured specific force resolved along body frame
2083 * y-axis and expressed in meters per squared
2084 * second (m/s^2).
2085 * @param fz measured specific force resolved along body frame
2086 * z-axis and expressed in meters per squared
2087 * second (m/s^2).
2088 * @param config Loosely Coupled Kalman filter configuration.
2089 * @return new state of Kalman filter.
2090 * @throws AlgebraException if there are numerical instabilities.
2091 */
2092 public static INSLooselyCoupledKalmanState estimate(
2093 final ECEFPositionAndVelocity positionAndVelocity, final Time propagationInterval,
2094 final INSLooselyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
2095 final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
2096 return estimate(positionAndVelocity.getX(), positionAndVelocity.getY(), positionAndVelocity.getZ(),
2097 positionAndVelocity.getVx(), positionAndVelocity.getVy(), positionAndVelocity.getVz(),
2098 propagationInterval, previousState, fx, fy, fz, config);
2099 }
2100
2101 /**
2102 * Estimates the update of Kalman filter state for a single epoch.
2103 *
2104 * @param positionAndVelocity ECEF user position and velocity.
2105 * @param propagationInterval propagation interval.
2106 * @param previousState previous Kalman filter state.
2107 * @param fx measured specific force resolved along body frame
2108 * x-axis and expressed in meters per squared
2109 * second (m/s^2).
2110 * @param fy measured specific force resolved along body frame
2111 * y-axis and expressed in meters per squared
2112 * second (m/s^2).
2113 * @param fz measured specific force resolved along body frame
2114 * z-axis and expressed in meters per squared
2115 * second (m/s^2).
2116 * @param config Loosely Coupled Kalman filter configuration.
2117 * @param result instance where new state of Kalman filter will be
2118 * stored.
2119 * @throws AlgebraException if there are numerical instabilities.
2120 */
2121 public static void estimate(
2122 final ECEFPositionAndVelocity positionAndVelocity, final Time propagationInterval,
2123 final INSLooselyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
2124 final INSLooselyCoupledKalmanConfig config, final INSLooselyCoupledKalmanState result)
2125 throws AlgebraException {
2126 estimate(positionAndVelocity.getX(), positionAndVelocity.getY(), positionAndVelocity.getZ(),
2127 positionAndVelocity.getVx(), positionAndVelocity.getVy(), positionAndVelocity.getVz(),
2128 propagationInterval, previousState, fx, fy, fz, config, result);
2129 }
2130
2131 /**
2132 * Estimates the update of Kalman filter state for a single epoch.
2133 *
2134 * @param positionAndVelocity ECEF user position and velocity.
2135 * @param propagationInterval propagation interval.
2136 * @param previousState previous Kalman filter state.
2137 * @param fx measured specific force resolved along body frame
2138 * x-axis and expressed in meters per squared
2139 * second (m/s^2).
2140 * @param fy measured specific force resolved along body frame
2141 * y-axis and expressed in meters per squared
2142 * second (m/s^2).
2143 * @param fz measured specific force resolved along body frame
2144 * z-axis and expressed in meters per squared
2145 * second (m/s^2).
2146 * @param previousLatitude previous latitude solution expressed in radians (rad).
2147 * @param config Loosely Coupled Kalman filter configuration.
2148 * @return new state of Kalman filter.
2149 * @throws AlgebraException if there are numerical instabilities.
2150 */
2151 public static INSLooselyCoupledKalmanState estimate(
2152 final ECEFPositionAndVelocity positionAndVelocity, final Time propagationInterval,
2153 final INSLooselyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
2154 final double previousLatitude, final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
2155 return estimate(positionAndVelocity.getX(), positionAndVelocity.getY(), positionAndVelocity.getZ(),
2156 positionAndVelocity.getVx(), positionAndVelocity.getVy(), positionAndVelocity.getVz(),
2157 propagationInterval, previousState, fx, fy, fz, previousLatitude, config);
2158 }
2159
2160 /**
2161 * Estimates the update of Kalman filter state for a single epoch.
2162 *
2163 * @param positionAndVelocity ECEF user position and velocity.
2164 * @param propagationInterval propagation interval.
2165 * @param previousState previous Kalman filter state.
2166 * @param fx measured specific force resolved along body frame
2167 * x-axis and expressed in meters per squared
2168 * second (m/s^2).
2169 * @param fy measured specific force resolved along body frame
2170 * y-axis and expressed in meters per squared
2171 * second (m/s^2).
2172 * @param fz measured specific force resolved along body frame
2173 * z-axis and expressed in meters per squared
2174 * second (m/s^2).
2175 * @param previousLatitude previous latitude solution expressed in radians (rad).
2176 * @param config Loosely Coupled Kalman filter configuration.
2177 * @param result instance where new state of Kalman filter will be
2178 * stored.
2179 * @throws AlgebraException if there are numerical instabilities.
2180 */
2181 public static void estimate(
2182 final ECEFPositionAndVelocity positionAndVelocity, final Time propagationInterval,
2183 final INSLooselyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
2184 final double previousLatitude, final INSLooselyCoupledKalmanConfig config,
2185 final INSLooselyCoupledKalmanState result) throws AlgebraException {
2186 estimate(positionAndVelocity.getX(), positionAndVelocity.getY(), positionAndVelocity.getZ(),
2187 positionAndVelocity.getVx(), positionAndVelocity.getVy(), positionAndVelocity.getVz(),
2188 propagationInterval, previousState, fx, fy, fz, previousLatitude, config, result);
2189 }
2190
2191 /**
2192 * Estimates the update of Kalman filter state for a single epoch.
2193 *
2194 * @param userPosition ECEF user position.
2195 * @param userVelocity ECEF user velocity.
2196 * @param propagationInterval propagation interval expressed in seconds (s).
2197 * @param previousState previous Kalman filter state.
2198 * @param bodyKinematics body kinematics containing measured specific force
2199 * resolved along body frame axes.
2200 * @param previousLatitude previous latitude solution.
2201 * @param config Loosely Coupled Kalman filter configuration.
2202 * @return new state of Kalman filter.
2203 * @throws AlgebraException if there are numerical instabilities.
2204 */
2205 public static INSLooselyCoupledKalmanState estimate(
2206 final ECEFPosition userPosition, final ECEFVelocity userVelocity, final double propagationInterval,
2207 final INSLooselyCoupledKalmanState previousState, final BodyKinematics bodyKinematics,
2208 final Angle previousLatitude, final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
2209 return estimate(userPosition, userVelocity, propagationInterval, previousState, bodyKinematics,
2210 convertAngle(previousLatitude), config);
2211 }
2212
2213 /**
2214 * Estimates the update of Kalman filter state for a single epoch.
2215 *
2216 * @param userPosition ECEF user position.
2217 * @param userVelocity ECEF user velocity.
2218 * @param propagationInterval propagation interval expressed in seconds (s).
2219 * @param previousState previous Kalman filter state.
2220 * @param bodyKinematics body kinematics containing measured specific force
2221 * resolved along body frame axes.
2222 * @param previousLatitude previous latitude solution.
2223 * @param config Loosely Coupled Kalman filter configuration.
2224 * @param result instance where new state of Kalman filter will be
2225 * stored.
2226 * @throws AlgebraException if there are numerical instabilities.
2227 */
2228 public static void estimate(
2229 final ECEFPosition userPosition, final ECEFVelocity userVelocity,
2230 final double propagationInterval, final INSLooselyCoupledKalmanState previousState,
2231 final BodyKinematics bodyKinematics, final Angle previousLatitude,
2232 final INSLooselyCoupledKalmanConfig config, final INSLooselyCoupledKalmanState result)
2233 throws AlgebraException {
2234 estimate(userPosition, userVelocity, propagationInterval, previousState, bodyKinematics,
2235 convertAngle(previousLatitude), config, result);
2236 }
2237
2238 /**
2239 * Estimates the update of Kalman filter state for a single epoch.
2240 *
2241 * @param userPosition ECEF user position.
2242 * @param userVelocity ECEF user velocity.
2243 * @param propagationInterval propagation interval expressed in seconds (s).
2244 * @param previousState previous Kalman filter state.
2245 * @param fx measured specific force resolved along body frame
2246 * x-axis and expressed in meters per squared
2247 * second (m/s^2).
2248 * @param fy measured specific force resolved along body frame
2249 * y-axis and expressed in meters per squared
2250 * second (m/s^2).
2251 * @param fz measured specific force resolved along body frame
2252 * z-axis and expressed in meters per squared
2253 * second (m/s^2).
2254 * @param previousLatitude previous latitude solution.
2255 * @param config Loosely Coupled Kalman filter configuration.
2256 * @return new state of Kalman filter.
2257 * @throws AlgebraException if there are numerical instabilities.
2258 */
2259 public static INSLooselyCoupledKalmanState estimate(
2260 final ECEFPosition userPosition, final ECEFVelocity userVelocity,
2261 final double propagationInterval, final INSLooselyCoupledKalmanState previousState,
2262 final double fx, final double fy, final double fz, final Angle previousLatitude,
2263 final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
2264 return estimate(userPosition, userVelocity, propagationInterval, previousState, fx, fy, fz,
2265 convertAngle(previousLatitude), config);
2266 }
2267
2268 /**
2269 * Estimates the update of Kalman filter state for a single epoch.
2270 *
2271 * @param userPosition GNSS estimated ECEF user position.
2272 * @param userVelocity GNSS estimated ECEF user velocity.
2273 * @param propagationInterval propagation interval expressed in seconds (s).
2274 * @param previousState previous Kalman filter state.
2275 * @param fx measured specific force resolved along body frame
2276 * x-axis and expressed in meters per squared
2277 * second (m/s^2).
2278 * @param fy measured specific force resolved along body frame
2279 * y-axis and expressed in meters per squared
2280 * second (m/s^2).
2281 * @param fz measured specific force resolved along body frame
2282 * z-axis and expressed in meters per squared
2283 * second (m/s^2).
2284 * @param previousLatitude previous latitude solution.
2285 * @param config Loosely Coupled Kalman filter configuration.
2286 * @param result instance where new state of Kalman filter will be
2287 * stored.
2288 * @throws AlgebraException if there are numerical instabilities.
2289 */
2290 public static void estimate(
2291 final ECEFPosition userPosition, final ECEFVelocity userVelocity,
2292 final double propagationInterval, final INSLooselyCoupledKalmanState previousState,
2293 final double fx, final double fy, final double fz, final Angle previousLatitude,
2294 final INSLooselyCoupledKalmanConfig config, final INSLooselyCoupledKalmanState result)
2295 throws AlgebraException {
2296 estimate(userPosition, userVelocity, propagationInterval, previousState, fx, fy, fz,
2297 convertAngle(previousLatitude), config, result);
2298 }
2299
2300 /**
2301 * Estimates the update of Kalman filter state for a single epoch.
2302 *
2303 * @param x ECEF x coordinate of user position expressed in
2304 * meters (m).
2305 * @param y ECEF y coordinate of user position expressed in
2306 * meters (m).
2307 * @param z ECEF z coordinate of user position expressed in
2308 * meters (m).
2309 * @param vx ECEF x coordinate of user velocity expressed in
2310 * meters per second (m/s).
2311 * @param vy ECEF y coordinate of user velocity expressed in
2312 * meters per second (m/s).
2313 * @param vz ECEF z coordinate of user velocity expressed in
2314 * meters per second (m/s).
2315 * @param propagationInterval propagation interval expressed in seconds (s).
2316 * @param previousState previous Kalman filter state.
2317 * @param bodyKinematics body kinematics containing measured specific force
2318 * resolved along body frame axes.
2319 * @param previousLatitude previous latitude solution.
2320 * @param config Loosely Coupled Kalman filter configuration.
2321 * @return new state of Kalman filter.
2322 * @throws AlgebraException if there are numerical instabilities.
2323 */
2324 public static INSLooselyCoupledKalmanState estimate(
2325 final double x, final double y, final double z, final double vx, final double vy, final double vz,
2326 final double propagationInterval, final INSLooselyCoupledKalmanState previousState,
2327 final BodyKinematics bodyKinematics, final Angle previousLatitude,
2328 final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
2329 return estimate(x, y, z, vx, vy, vz, propagationInterval, previousState, bodyKinematics,
2330 convertAngle(previousLatitude), config);
2331 }
2332
2333 /**
2334 * Estimates the update of Kalman filter state for a single epoch.
2335 *
2336 * @param x ECEF x coordinate of user position expressed in
2337 * meters (m).
2338 * @param y ECEF y coordinate of user position expressed in
2339 * meters (m).
2340 * @param z ECEF z coordinate of user position expressed in
2341 * meters (m).
2342 * @param vx ECEF x coordinate of user velocity expressed in
2343 * meters per second (m/s).
2344 * @param vy ECEF y coordinate of user velocity expressed in
2345 * meters per second (m/s).
2346 * @param vz ECEF z coordinate of user velocity expressed in
2347 * meters per second (m/s).
2348 * @param propagationInterval propagation interval expressed in seconds (s).
2349 * @param previousState previous Kalman filter state.
2350 * @param bodyKinematics body kinematics containing measured specific force
2351 * resolved along body frame axes.
2352 * @param previousLatitude previous latitude solution.
2353 * @param config Loosely Coupled Kalman filter configuration.
2354 * @param result instance where new state of Kalman filter will be
2355 * stored.
2356 * @throws AlgebraException if there are numerical instabilities.
2357 */
2358 public static void estimate(
2359 final double x, final double y, final double z, final double vx, final double vy, final double vz,
2360 final double propagationInterval, final INSLooselyCoupledKalmanState previousState,
2361 final BodyKinematics bodyKinematics, final Angle previousLatitude,
2362 final INSLooselyCoupledKalmanConfig config, final INSLooselyCoupledKalmanState result)
2363 throws AlgebraException {
2364 estimate(x, y, z, vx, vy, vz, propagationInterval, previousState, bodyKinematics,
2365 convertAngle(previousLatitude), config, result);
2366 }
2367
2368 /**
2369 * Estimates the update of Kalman filter state for a single epoch.
2370 *
2371 * @param x ECEF x coordinate of user position expressed in
2372 * meters (m).
2373 * @param y ECEF y coordinate of user position expressed in
2374 * meters (m).
2375 * @param z ECEF z coordinate of user position expressed in
2376 * meters (m).
2377 * @param vx ECEF x coordinate of user velocity expressed in
2378 * meters per second (m/s).
2379 * @param vy ECEF y coordinate of user velocity expressed in
2380 * meters per second (m/s).
2381 * @param vz ECEF z coordinate of user velocity expressed in
2382 * meters per second (m/s).
2383 * @param propagationInterval propagation interval expressed in seconds (s).
2384 * @param previousState previous Kalman filter state.
2385 * @param fx measured specific force resolved along body frame
2386 * x-axis and expressed in meters per squared
2387 * second (m/s^2).
2388 * @param fy measured specific force resolved along body frame
2389 * y-axis and expressed in meters per squared
2390 * second (m/s^2).
2391 * @param fz measured specific force resolved along body frame
2392 * z-axis and expressed in meters per squared
2393 * second (m/s^2).
2394 * @param previousLatitude previous latitude solution.
2395 * @param config Loosely Coupled Kalman filter configuration.
2396 * @return new state of Kalman filter.
2397 * @throws AlgebraException if there are numerical instabilities.
2398 */
2399 public static INSLooselyCoupledKalmanState estimate(
2400 final double x, final double y, final double z, final double vx, final double vy, final double vz,
2401 final double propagationInterval, final INSLooselyCoupledKalmanState previousState,
2402 final double fx, final double fy, final double fz, final Angle previousLatitude,
2403 final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
2404 return estimate(x, y, z, vx, vy, vz, propagationInterval, previousState, fx, fy, fz,
2405 convertAngle(previousLatitude), config);
2406 }
2407
2408 /**
2409 * Estimates the update of Kalman filter state for a single epoch.
2410 *
2411 * @param x ECEF x coordinate of user position expressed in
2412 * meters (m).
2413 * @param y ECEF y coordinate of user position expressed in
2414 * meters (m).
2415 * @param z ECEF z coordinate of user position expressed in
2416 * meters (m).
2417 * @param vx ECEF x coordinate of user velocity expressed in
2418 * meters per second (m/s).
2419 * @param vy ECEF y coordinate of user velocity expressed in
2420 * meters per second (m/s).
2421 * @param vz ECEF z coordinate of user velocity expressed in
2422 * meters per second (m/s).
2423 * @param propagationInterval propagation interval expressed in seconds (s).
2424 * @param previousState previous Kalman filter state.
2425 * @param fx measured specific force resolved along body frame
2426 * x-axis and expressed in meters per squared
2427 * second (m/s^2).
2428 * @param fy measured specific force resolved along body frame
2429 * y-axis and expressed in meters per squared
2430 * second (m/s^2).
2431 * @param fz measured specific force resolved along body frame
2432 * z-axis and expressed in meters per squared
2433 * second (m/s^2).
2434 * @param previousLatitude previous latitude solution.
2435 * @param config Loosely Coupled Kalman filter configuration.
2436 * @param result instance where new state of Kalman filter will be
2437 * stored.
2438 * @throws AlgebraException if there are numerical instabilities.
2439 */
2440 public static void estimate(
2441 final double x, final double y, final double z, final double vx, final double vy, final double vz,
2442 final double propagationInterval, final INSLooselyCoupledKalmanState previousState,
2443 final double fx, final double fy, final double fz, final Angle previousLatitude,
2444 final INSLooselyCoupledKalmanConfig config, final INSLooselyCoupledKalmanState result)
2445 throws AlgebraException {
2446 estimate(x, y, z, vx, vy, vz, propagationInterval, previousState, fx, fy, fz, convertAngle(previousLatitude),
2447 config, result);
2448 }
2449
2450 /**
2451 * Estimates the update of Kalman filter state for a single epoch.
2452 *
2453 * @param userPosition ECEF user position.
2454 * @param userVelocity ECEF user velocity.
2455 * @param propagationInterval propagation interval.
2456 * @param previousState previous Kalman filter state.
2457 * @param bodyKinematics body kinematics containing measured specific force
2458 * resolved along body frame axes.
2459 * @param previousLatitude previous latitude solution.
2460 * @param config Loosely Coupled Kalman filter configuration.
2461 * @return new state of Kalman filter.
2462 * @throws AlgebraException if there are numerical instabilities.
2463 */
2464 public static INSLooselyCoupledKalmanState estimate(
2465 final ECEFPosition userPosition, final ECEFVelocity userVelocity, final Time propagationInterval,
2466 final INSLooselyCoupledKalmanState previousState, final BodyKinematics bodyKinematics,
2467 final Angle previousLatitude, final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
2468 return estimate(userPosition, userVelocity, propagationInterval, previousState, bodyKinematics,
2469 convertAngle(previousLatitude), config);
2470 }
2471
2472 /**
2473 * Estimates the update of Kalman filter state of a single epoch.
2474 *
2475 * @param userPosition ECEF user position.
2476 * @param userVelocity ECEF user velocity.
2477 * @param propagationInterval propagation interval.
2478 * @param previousState previous Kalman filter state.
2479 * @param bodyKinematics body kinematics containing measured specific force
2480 * resolved along body frame axes.
2481 * @param previousLatitude previous latitude solution.
2482 * @param config Loosely Coupled Kalman filter configuration.
2483 * @param result instance where new state of Kalman filter will be
2484 * stored.
2485 * @throws AlgebraException if there are numerical instabilities.
2486 */
2487 public static void estimate(
2488 final ECEFPosition userPosition, final ECEFVelocity userVelocity,
2489 final Time propagationInterval, final INSLooselyCoupledKalmanState previousState,
2490 final BodyKinematics bodyKinematics, final Angle previousLatitude,
2491 final INSLooselyCoupledKalmanConfig config, final INSLooselyCoupledKalmanState result)
2492 throws AlgebraException {
2493 estimate(userPosition, userVelocity, propagationInterval, previousState, bodyKinematics,
2494 convertAngle(previousLatitude), config, result);
2495 }
2496
2497 /**
2498 * Estimates the update of Kalman filter state of a single epoch.
2499 *
2500 * @param userPosition ECEF user position.
2501 * @param userVelocity ECEF user velocity.
2502 * @param propagationInterval propagation interval.
2503 * @param previousState previous Kalman filter state.
2504 * @param fx measured specific force resolved along body frame
2505 * x-axis and expressed in meters per squared
2506 * second (m/s^2).
2507 * @param fy measured specific force resolved along body frame
2508 * y-axis and expressed in meters per squared
2509 * second (m/s^2).
2510 * @param fz measured specific force resolved along body frame
2511 * z-axis and expressed in meters per squared
2512 * second (m/s^2).
2513 * @param previousLatitude previous latitude solution.
2514 * @param config Loosely Coupled Kalman filter configuration.
2515 * @return new state of Kalman filter.
2516 * @throws AlgebraException if there are numerical instabilities.
2517 */
2518 public static INSLooselyCoupledKalmanState estimate(
2519 final ECEFPosition userPosition, final ECEFVelocity userVelocity, final Time propagationInterval,
2520 final INSLooselyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
2521 final Angle previousLatitude, final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
2522 return estimate(userPosition, userVelocity, propagationInterval, previousState, fx, fy, fz,
2523 convertAngle(previousLatitude), config);
2524 }
2525
2526 /**
2527 * Estimates the update of Kalman filter state of a single epoch.
2528 *
2529 * @param userPosition ECEF user position.
2530 * @param userVelocity ECEF user velocity.
2531 * @param propagationInterval propagation interval.
2532 * @param previousState previous Kalman filter state.
2533 * @param fx measured specific force resolved along body frame
2534 * x-axis and expressed in meters per squared
2535 * second (m/s^2).
2536 * @param fy measured specific force resolved along body frame
2537 * y-axis and expressed in meters per squared
2538 * second (m/s^2).
2539 * @param fz measured specific force resolved along body frame
2540 * z-axis and expressed in meters per squared
2541 * second (m/s^2).
2542 * @param previousLatitude previous latitude solution.
2543 * @param config Loosely Coupled Kalman filter configuration.
2544 * @param result instance where new state of Kalman filter will be
2545 * stored.
2546 * @throws AlgebraException if there are numerical instabilities.
2547 */
2548 public static void estimate(
2549 final ECEFPosition userPosition, final ECEFVelocity userVelocity, final Time propagationInterval,
2550 final INSLooselyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
2551 final Angle previousLatitude, final INSLooselyCoupledKalmanConfig config,
2552 final INSLooselyCoupledKalmanState result) throws AlgebraException {
2553 estimate(userPosition, userVelocity, propagationInterval, previousState, fx, fy, fz,
2554 convertAngle(previousLatitude), config, result);
2555 }
2556
2557 /**
2558 * Estimates the update of Kalman filter state for a single epoch.
2559 *
2560 * @param x ECEF x coordinate of user position expressed in
2561 * meters (m).
2562 * @param y ECEF y coordinate of user position expressed in
2563 * meters (m).
2564 * @param z ECEF z coordinate of user position expressed in
2565 * meters (m).
2566 * @param vx ECEF x coordinate of user velocity expressed in
2567 * meters per second (m/s).
2568 * @param vy ECEF y coordinate of user velocity expressed in
2569 * meters per second (m/s).
2570 * @param vz ECEF z coordinate of user velocity expressed in
2571 * meters per second (m/s).
2572 * @param propagationInterval propagation interval.
2573 * @param previousState previous Kalman filter state.
2574 * @param bodyKinematics body kinematics containing measured specific force
2575 * resolved along body frame axes.
2576 * @param previousLatitude previous latitude solution.
2577 * @param config Loosely Coupled Kalman filter configuration.
2578 * @return new state of Kalman filter.
2579 * @throws AlgebraException if there are numerical instabilities.
2580 */
2581 public static INSLooselyCoupledKalmanState estimate(
2582 final double x, final double y, final double z, final double vx, final double vy, final double vz,
2583 final Time propagationInterval, final INSLooselyCoupledKalmanState previousState,
2584 final BodyKinematics bodyKinematics, final Angle previousLatitude,
2585 final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
2586 return estimate(x, y, z, vx, vy, vz, propagationInterval, previousState, bodyKinematics,
2587 convertAngle(previousLatitude), config);
2588 }
2589
2590 /**
2591 * Estimates the update of Kalman filter state for a single epoch.
2592 *
2593 * @param x ECEF x coordinate of user position expressed in
2594 * meters (m).
2595 * @param y ECEF y coordinate of user position expressed in
2596 * meters (m).
2597 * @param z ECEF z coordinate of user position expressed in
2598 * meters (m).
2599 * @param vx ECEF x coordinate of user velocity expressed in
2600 * meters per second (m/s).
2601 * @param vy ECEF y coordinate of user velocity expressed in
2602 * meters per second (m/s).
2603 * @param vz ECEF z coordinate of user velocity expressed in
2604 * meters per second (m/s).
2605 * @param propagationInterval propagation interval.
2606 * @param previousState previous Kalman filter state.
2607 * @param bodyKinematics body kinematics containing measured specific force
2608 * resolved along body frame axes.
2609 * @param previousLatitude previous latitude solution.
2610 * @param config Loosely Coupled Kalman filter configuration.
2611 * @param result instance where new state of Kalman filter will be
2612 * stored.
2613 * @throws AlgebraException if there are numerical instabilities.
2614 */
2615 public static void estimate(
2616 final double x, final double y, final double z, final double vx, final double vy, final double vz,
2617 final Time propagationInterval, final INSLooselyCoupledKalmanState previousState,
2618 final BodyKinematics bodyKinematics, final Angle previousLatitude,
2619 final INSLooselyCoupledKalmanConfig config, final INSLooselyCoupledKalmanState result)
2620 throws AlgebraException {
2621 estimate(x, y, z, vx, vy, vz, propagationInterval, previousState, bodyKinematics,
2622 convertAngle(previousLatitude), config, result);
2623 }
2624
2625 /**
2626 * Estimates the update of Kalman filter state for a single epoch.
2627 *
2628 * @param x ECEF x coordinate of user position expressed in
2629 * meters (m).
2630 * @param y ECEF y coordinate of user position expressed in
2631 * meters (m).
2632 * @param z ECEF z coordinate of user position expressed in
2633 * meters (m).
2634 * @param vx ECEF x coordinate of user velocity expressed in
2635 * meters per second (m/s).
2636 * @param vy ECEF y coordinate of user velocity expressed in
2637 * meters per second (m/s).
2638 * @param vz ECEF z coordinate of user velocity expressed in
2639 * meters per second (m/s).
2640 * @param propagationInterval propagation interval.
2641 * @param previousState previous Kalman filter state.
2642 * @param fx measured specific force resolved along body frame
2643 * x-axis and expressed in meters per squared
2644 * second (m/s^2).
2645 * @param fy measured specific force resolved along body frame
2646 * y-axis and expressed in meters per squared
2647 * second (m/s^2).
2648 * @param fz measured specific force resolved along body frame
2649 * z-axis and expressed in meters per squared
2650 * second (m/s^2).
2651 * @param previousLatitude previous latitude solution.
2652 * @param config Loosely Coupled Kalman filter configuration.
2653 * @return new state of Kalman filter.
2654 * @throws AlgebraException if there are numerical instabilities.
2655 */
2656 public static INSLooselyCoupledKalmanState estimate(
2657 final double x, final double y, final double z, final double vx, final double vy, final double vz,
2658 final Time propagationInterval, final INSLooselyCoupledKalmanState previousState,
2659 final double fx, final double fy, final double fz, final Angle previousLatitude,
2660 final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
2661 return estimate(x, y, z, vx, vy, vz, propagationInterval, previousState, fx, fy, fz,
2662 convertAngle(previousLatitude), config);
2663 }
2664
2665 /**
2666 * Estimates the update of Kalman filter state for a single epoch.
2667 *
2668 * @param x ECEF x coordinate of user position expressed in
2669 * meters (m).
2670 * @param y ECEF y coordinate of user position expressed in
2671 * meters (m).
2672 * @param z ECEF z coordinate of user position expressed in
2673 * meters (m).
2674 * @param vx ECEF x coordinate of user velocity expressed in
2675 * meters per second (m/s).
2676 * @param vy ECEF y coordinate of user velocity expressed in
2677 * meters per second (m/s).
2678 * @param vz ECEF z coordinate of user velocity expressed in
2679 * meters per second (m/s).
2680 * @param propagationInterval propagation interval.
2681 * @param previousState previous Kalman filter state.
2682 * @param fx measured specific force resolved along body frame
2683 * x-axis and expressed in meters per squared
2684 * second (m/s^2).
2685 * @param fy measured specific force resolved along body frame
2686 * y-axis and expressed in meters per squared
2687 * second (m/s^2).
2688 * @param fz measured specific force resolved along body frame
2689 * z-axis and expressed in meters per squared
2690 * second (m/s^2).
2691 * @param previousLatitude previous latitude solution.
2692 * @param config Loosely Coupled Kalman filter configuration.
2693 * @param result instance where new state of Kalman filter will be
2694 * stored.
2695 * @throws AlgebraException if there are numerical instabilities.
2696 */
2697 public static void estimate(
2698 final double x, final double y, final double z, final double vx, final double vy, final double vz,
2699 final Time propagationInterval, final INSLooselyCoupledKalmanState previousState,
2700 final double fx, final double fy, final double fz, final Angle previousLatitude,
2701 final INSLooselyCoupledKalmanConfig config, final INSLooselyCoupledKalmanState result)
2702 throws AlgebraException {
2703 estimate(x, y, z, vx, vy, vz, propagationInterval, previousState, fx, fy, fz, convertAngle(previousLatitude),
2704 config, result);
2705 }
2706
2707 /**
2708 * Estimates the update of Kalman filter state for a single epoch.
2709 *
2710 * @param userPosition ECEF user position expressed in meters (m).
2711 * @param userVelocity ECEF user velocity.
2712 * @param propagationInterval propagation interval expressed in seconds (s).
2713 * @param previousState previous Kalman filter state.
2714 * @param bodyKinematics body kinematics containing measured specific force
2715 * resolved along body frame axes.
2716 * @param previousLatitude previous latitude solution.
2717 * @param config Loosely Coupled Kalman filter configuration.
2718 * @return new state of Kalman filter.
2719 * @throws AlgebraException if there are numerical instabilities.
2720 */
2721 public static INSLooselyCoupledKalmanState estimate(
2722 final Point3D userPosition, final ECEFVelocity userVelocity, final double propagationInterval,
2723 final INSLooselyCoupledKalmanState previousState, final BodyKinematics bodyKinematics,
2724 final Angle previousLatitude, final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
2725 return estimate(userPosition, userVelocity, propagationInterval, previousState, bodyKinematics,
2726 convertAngle(previousLatitude), config);
2727 }
2728
2729 /**
2730 * Estimates the update of Kalman filter state for a single epoch.
2731 *
2732 * @param userPosition ECEF user position expressed in meters (m).
2733 * @param userVelocity ECEF user velocity.
2734 * @param propagationInterval propagation interval expressed in seconds (s).
2735 * @param previousState previous Kalman filter state.
2736 * @param bodyKinematics body kinematics containing measured specific force
2737 * resolved along body frame axes.
2738 * @param previousLatitude previous latitude solution expressed in radians (rad).
2739 * @param config Loosely Coupled Kalman filter configuration.
2740 * @param result instance where new state of Kalman filter will be
2741 * stored.
2742 * @throws AlgebraException if there are numerical instabilities.
2743 */
2744 public static void estimate(
2745 final Point3D userPosition, final ECEFVelocity userVelocity, final double propagationInterval,
2746 final INSLooselyCoupledKalmanState previousState, final BodyKinematics bodyKinematics,
2747 final Angle previousLatitude, final INSLooselyCoupledKalmanConfig config,
2748 final INSLooselyCoupledKalmanState result) throws AlgebraException {
2749 estimate(userPosition, userVelocity, propagationInterval, previousState, bodyKinematics,
2750 convertAngle(previousLatitude), config, result);
2751 }
2752
2753 /**
2754 * Estimates the update of Kalman filter state for a single epoch.
2755 *
2756 * @param userPosition ECEF user position expressed in meters (m).
2757 * @param userVelocity ECEF user velocity.
2758 * @param propagationInterval propagation interval expressed in seconds (s).
2759 * @param previousState previous Kalman filter state.
2760 * @param fx measured specific force resolved along body frame
2761 * x-axis and expressed in meters per squared
2762 * second (m/s^2).
2763 * @param fy measured specific force resolved along body frame
2764 * y-axis and expressed in meters per squared
2765 * second (m/s^2).
2766 * @param fz measured specific force resolved along body frame
2767 * z-axis and expressed in meters per squared
2768 * second (m/s^2).
2769 * @param previousLatitude previous latitude solution.
2770 * @param config Loosely Coupled Kalman filter configuration.
2771 * @return new state of Kalman filter.
2772 * @throws AlgebraException if there are numerical instabilities.
2773 */
2774 public static INSLooselyCoupledKalmanState estimate(
2775 final Point3D userPosition, final ECEFVelocity userVelocity, final double propagationInterval,
2776 final INSLooselyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
2777 final Angle previousLatitude, final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
2778 return estimate(userPosition, userVelocity, propagationInterval, previousState, fx, fy, fz,
2779 convertAngle(previousLatitude), config);
2780 }
2781
2782 /**
2783 * Estimates the update of Kalman filter state for a single epoch.
2784 *
2785 * @param userPosition ECEF user position expressed in meters (m).
2786 * @param userVelocity ECEF user velocity.
2787 * @param propagationInterval propagation interval expressed in seconds (s).
2788 * @param previousState previous Kalman filter state.
2789 * @param fx measured specific force resolved along body frame
2790 * x-axis and expressed in meters per squared
2791 * second (m/s^2).
2792 * @param fy measured specific force resolved along body frame
2793 * y-axis and expressed in meters per squared
2794 * second (m/s^2).
2795 * @param fz measured specific force resolved along body frame
2796 * z-axis and expressed in meters per squared
2797 * second (m/s^2).
2798 * @param previousLatitude previous latitude solution.
2799 * @param config Loosely Coupled Kalman filter configuration.
2800 * @param result instance where new state of Kalman filter will be
2801 * stored.
2802 * @throws AlgebraException if there are numerical instabilities.
2803 */
2804 public static void estimate(
2805 final Point3D userPosition, final ECEFVelocity userVelocity, final double propagationInterval,
2806 final INSLooselyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
2807 final Angle previousLatitude, final INSLooselyCoupledKalmanConfig config,
2808 final INSLooselyCoupledKalmanState result) throws AlgebraException {
2809 estimate(userPosition, userVelocity, propagationInterval, previousState, fx, fy, fz,
2810 convertAngle(previousLatitude), config, result);
2811 }
2812
2813 /**
2814 * Estimates the update of Kalman filter state for a single epoch.
2815 *
2816 * @param userPosition ECEF user position expressed in meters (m).
2817 * @param userVelocity ECEF user velocity.
2818 * @param propagationInterval propagation interval.
2819 * @param previousState previous Kalman filter state.
2820 * @param bodyKinematics body kinematics containing measured specific force
2821 * resolved along body frame axes.
2822 * @param previousLatitude previous latitude solution.
2823 * @param config Loosely Coupled Kalman filter configuration.
2824 * @return new state of Kalman filter.
2825 * @throws AlgebraException if there are numerical instabilities.
2826 */
2827 public static INSLooselyCoupledKalmanState estimate(
2828 final Point3D userPosition, final ECEFVelocity userVelocity, final Time propagationInterval,
2829 final INSLooselyCoupledKalmanState previousState, final BodyKinematics bodyKinematics,
2830 final Angle previousLatitude, final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
2831 return estimate(userPosition, userVelocity, propagationInterval, previousState, bodyKinematics,
2832 convertAngle(previousLatitude), config);
2833 }
2834
2835 /**
2836 * Estimates the update of Kalman filter state for a single epoch.
2837 *
2838 * @param userPosition ECEF user position expressed in meters (m).
2839 * @param userVelocity ECEF user velocity.
2840 * @param propagationInterval propagation interval.
2841 * @param previousState previous Kalman filter state.
2842 * @param bodyKinematics body kinematics containing measured specific force
2843 * resolved along body frame axes.
2844 * @param previousLatitude previous latitude solution.
2845 * @param config Loosely Coupled Kalman filter configuration.
2846 * @param result instance where new state of Kalman filter will be
2847 * stored.
2848 * @throws AlgebraException if there are numerical instabilities.
2849 */
2850 public static void estimate(
2851 final Point3D userPosition, final ECEFVelocity userVelocity, final Time propagationInterval,
2852 final INSLooselyCoupledKalmanState previousState, final BodyKinematics bodyKinematics,
2853 final Angle previousLatitude, final INSLooselyCoupledKalmanConfig config,
2854 final INSLooselyCoupledKalmanState result) throws AlgebraException {
2855 estimate(userPosition, userVelocity, propagationInterval, previousState, bodyKinematics,
2856 convertAngle(previousLatitude), config, result);
2857 }
2858
2859 /**
2860 * Estimates the update of Kalman filter state for a single epoch.
2861 *
2862 * @param userPosition ECEF user position expressed in meters (m).
2863 * @param userVelocity ECEF user velocity.
2864 * @param propagationInterval propagation interval.
2865 * @param previousState previous Kalman filter state.
2866 * @param fx measured specific force resolved along body frame
2867 * x-axis and expressed in meters per squared
2868 * second (m/s^2).
2869 * @param fy measured specific force resolved along body frame
2870 * y-axis and expressed in meters per squared
2871 * second (m/s^2).
2872 * @param fz measured specific force resolved along body frame
2873 * z-axis and expressed in meters per squared
2874 * second (m/s^2).
2875 * @param previousLatitude previous latitude solution.
2876 * @param config Loosely Coupled Kalman filter configuration.
2877 * @return new state of Kalman filter.
2878 * @throws AlgebraException if there are numerical instabilities.
2879 */
2880 public static INSLooselyCoupledKalmanState estimate(
2881 final Point3D userPosition, final ECEFVelocity userVelocity, final Time propagationInterval,
2882 final INSLooselyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
2883 final Angle previousLatitude, final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
2884 return estimate(userPosition, userVelocity, propagationInterval, previousState, fx, fy, fz,
2885 convertAngle(previousLatitude), config);
2886 }
2887
2888 /**
2889 * Estimates the update of Kalman filter state for a single epoch.
2890 *
2891 * @param userPosition ECEF user position expressed in meters (m).
2892 * @param userVelocity ECEF user velocity.
2893 * @param propagationInterval propagation interval.
2894 * @param previousState previous Kalman filter state.
2895 * @param fx measured specific force resolved along body frame
2896 * x-axis and expressed in meters per squared
2897 * second (m/s^2).
2898 * @param fy measured specific force resolved along body frame
2899 * y-axis and expressed in meters per squared
2900 * second (m/s^2).
2901 * @param fz measured specific force resolved along body frame
2902 * z-axis and expressed in meters per squared
2903 * second (m/s^2).
2904 * @param previousLatitude previous latitude solution.
2905 * @param config Loosely Coupled Kalman filter configuration.
2906 * @param result instance where new state of Kalman filter will be
2907 * stored.
2908 * @throws AlgebraException if there are numerical instabilities.
2909 */
2910 public static void estimate(
2911 final Point3D userPosition, final ECEFVelocity userVelocity, final Time propagationInterval,
2912 final INSLooselyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
2913 final Angle previousLatitude, final INSLooselyCoupledKalmanConfig config,
2914 final INSLooselyCoupledKalmanState result) throws AlgebraException {
2915 estimate(userPosition, userVelocity, propagationInterval, previousState, fx, fy, fz,
2916 convertAngle(previousLatitude), config, result);
2917 }
2918
2919 /**
2920 * Estimates the update of Kalman filter state for a single epoch.
2921 *
2922 * @param positionAndVelocity ECEF user position and velocity.
2923 * @param propagationInterval propagation interval expressed in seconds (s).
2924 * @param previousState previous Kalman filter state.
2925 * @param bodyKinematics body kinematics containing measured specific force
2926 * resolved along body frame axes.
2927 * @param previousLatitude previous latitude solution.
2928 * @param config Loosely Coupled Kalman filter configuration.
2929 * @return new state of Kalman filter.
2930 * @throws AlgebraException if there are numerical instabilities.
2931 */
2932 public static INSLooselyCoupledKalmanState estimate(
2933 final ECEFPositionAndVelocity positionAndVelocity, final double propagationInterval,
2934 final INSLooselyCoupledKalmanState previousState, final BodyKinematics bodyKinematics,
2935 final Angle previousLatitude, final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
2936 return estimate(positionAndVelocity, propagationInterval, previousState, bodyKinematics,
2937 convertAngle(previousLatitude), config);
2938 }
2939
2940 /**
2941 * Estimates the update of Kalman filter state for a single epoch.
2942 *
2943 * @param positionAndVelocity ECEF user position and velocity.
2944 * @param propagationInterval propagation interval expressed in seconds (s).
2945 * @param previousState previous Kalman filter state.
2946 * @param bodyKinematics body kinematics containing measured specific force
2947 * resolved along body frame axes.
2948 * @param previousLatitude previous latitude solution.
2949 * @param config Loosely Coupled Kalman filter configuration.
2950 * @param result instance where new state of Kalman filter will be
2951 * stored.
2952 * @throws AlgebraException if there are numerical instabilities.
2953 */
2954 public static void estimate(
2955 final ECEFPositionAndVelocity positionAndVelocity, final double propagationInterval,
2956 final INSLooselyCoupledKalmanState previousState, final BodyKinematics bodyKinematics,
2957 final Angle previousLatitude, final INSLooselyCoupledKalmanConfig config,
2958 final INSLooselyCoupledKalmanState result) throws AlgebraException {
2959 estimate(positionAndVelocity, propagationInterval, previousState, bodyKinematics,
2960 convertAngle(previousLatitude), config, result);
2961 }
2962
2963 /**
2964 * Estimates the update of Kalman filter state for a single epoch.
2965 *
2966 * @param positionAndVelocity ECEF user position and velocity.
2967 * @param propagationInterval propagation interval expressed in seconds (s).
2968 * @param previousState previous Kalman filter state.
2969 * @param fx measured specific force resolved along body frame
2970 * x-axis and expressed in meters per squared
2971 * second (m/s^2).
2972 * @param fy measured specific force resolved along body frame
2973 * y-axis and expressed in meters per squared
2974 * second (m/s^2).
2975 * @param fz measured specific force resolved along body frame
2976 * z-axis and expressed in meters per squared
2977 * second (m/s^2).
2978 * @param previousLatitude previous latitude solution.
2979 * @param config Loosely Coupled Kalman filter configuration.
2980 * @return new state of Kalman filter.
2981 * @throws AlgebraException if there are numerical instabilities.
2982 */
2983 public static INSLooselyCoupledKalmanState estimate(
2984 final ECEFPositionAndVelocity positionAndVelocity, final double propagationInterval,
2985 final INSLooselyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
2986 final Angle previousLatitude, final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
2987 return estimate(positionAndVelocity, propagationInterval, previousState, fx, fy, fz,
2988 convertAngle(previousLatitude), config);
2989 }
2990
2991 /**
2992 * Estimates the update of Kalman filter state for a single epoch.
2993 *
2994 * @param positionAndVelocity ECEF user position and velocity.
2995 * @param propagationInterval propagation interval expressed in seconds (s).
2996 * @param previousState previous Kalman filter state.
2997 * @param fx measured specific force resolved along body frame
2998 * x-axis and expressed in meters per squared
2999 * second (m/s^2).
3000 * @param fy measured specific force resolved along body frame
3001 * y-axis and expressed in meters per squared
3002 * second (m/s^2).
3003 * @param fz measured specific force resolved along body frame
3004 * z-axis and expressed in meters per squared
3005 * second (m/s^2).
3006 * @param previousLatitude previous latitude solution.
3007 * @param config Loosely Coupled Kalman filter configuration.
3008 * @param result instance where new state of Kalman filter will be
3009 * stored.
3010 * @throws AlgebraException if there are numerical instabilities.
3011 */
3012 public static void estimate(
3013 final ECEFPositionAndVelocity positionAndVelocity, final double propagationInterval,
3014 final INSLooselyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
3015 final Angle previousLatitude, final INSLooselyCoupledKalmanConfig config,
3016 final INSLooselyCoupledKalmanState result) throws AlgebraException {
3017 estimate(positionAndVelocity, propagationInterval, previousState, fx, fy, fz, convertAngle(previousLatitude),
3018 config, result);
3019 }
3020
3021 /**
3022 * Estimates the update of Kalman filter state for a single epoch.
3023 *
3024 * @param positionAndVelocity ECEF user position and velocity.
3025 * @param propagationInterval propagation interval.
3026 * @param previousState previous Kalman filter state.
3027 * @param bodyKinematics body kinematics containing measured specific force
3028 * resolved along body frame axes.
3029 * @param previousLatitude previous latitude solution.
3030 * @param config Loosely Coupled Kalman filter configuration.
3031 * @return new state of Kalman filter.
3032 * @throws AlgebraException if there are numerical instabilities.
3033 */
3034 public static INSLooselyCoupledKalmanState estimate(
3035 final ECEFPositionAndVelocity positionAndVelocity, final Time propagationInterval,
3036 final INSLooselyCoupledKalmanState previousState, final BodyKinematics bodyKinematics,
3037 final Angle previousLatitude, final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
3038 return estimate(positionAndVelocity, propagationInterval, previousState, bodyKinematics,
3039 convertAngle(previousLatitude), config);
3040 }
3041
3042 /**
3043 * Estimates the update of Kalman filter state for a single epoch.
3044 *
3045 * @param positionAndVelocity ECEF user position and velocity.
3046 * @param propagationInterval propagation interval.
3047 * @param previousState previous Kalman filter state.
3048 * @param bodyKinematics body kinematics containing measured specific force
3049 * resolved along body frame axes.
3050 * @param previousLatitude previous latitude solution.
3051 * @param config Loosely Coupled Kalman filter configuration.
3052 * @param result instance where new state of Kalman filter will be
3053 * stored.
3054 * @throws AlgebraException if there are numerical instabilities.
3055 */
3056 public static void estimate(
3057 final ECEFPositionAndVelocity positionAndVelocity, final Time propagationInterval,
3058 final INSLooselyCoupledKalmanState previousState, final BodyKinematics bodyKinematics,
3059 final Angle previousLatitude, final INSLooselyCoupledKalmanConfig config,
3060 final INSLooselyCoupledKalmanState result) throws AlgebraException {
3061 estimate(positionAndVelocity, propagationInterval, previousState, bodyKinematics,
3062 convertAngle(previousLatitude), config, result);
3063 }
3064
3065 /**
3066 * Estimates the update of Kalman filter state for a single epoch.
3067 *
3068 * @param positionAndVelocity ECEF user position and velocity.
3069 * @param propagationInterval propagation interval.
3070 * @param previousState previous Kalman filter state.
3071 * @param fx measured specific force resolved along body frame
3072 * x-axis and expressed in meters per squared
3073 * second (m/s^2).
3074 * @param fy measured specific force resolved along body frame
3075 * y-axis and expressed in meters per squared
3076 * second (m/s^2).
3077 * @param fz measured specific force resolved along body frame
3078 * z-axis and expressed in meters per squared
3079 * second (m/s^2).
3080 * @param previousLatitude previous latitude solution.
3081 * @param config Loosely Coupled Kalman filter configuration.
3082 * @return new state of Kalman filter.
3083 * @throws AlgebraException if there are numerical instabilities.
3084 */
3085 public static INSLooselyCoupledKalmanState estimate(
3086 final ECEFPositionAndVelocity positionAndVelocity, final Time propagationInterval,
3087 final INSLooselyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
3088 final Angle previousLatitude, final INSLooselyCoupledKalmanConfig config) throws AlgebraException {
3089 return estimate(positionAndVelocity, propagationInterval, previousState, fx, fy, fz,
3090 convertAngle(previousLatitude), config);
3091 }
3092
3093 /**
3094 * Estimates the update of Kalman filter state for a single epoch.
3095 *
3096 * @param positionAndVelocity ECEF user position and velocity.
3097 * @param propagationInterval propagation interval.
3098 * @param previousState previous Kalman filter state.
3099 * @param fx measured specific force resolved along body frame
3100 * x-axis and expressed in meters per squared
3101 * second (m/s^2).
3102 * @param fy measured specific force resolved along body frame
3103 * y-axis and expressed in meters per squared
3104 * second (m/s^2).
3105 * @param fz measured specific force resolved along body frame
3106 * z-axis and expressed in meters per squared
3107 * second (m/s^2).
3108 * @param previousLatitude previous latitude solution.
3109 * @param config Loosely Coupled Kalman filter configuration.
3110 * @param result instance where new state of Kalman filter will be
3111 * stored.
3112 * @throws AlgebraException if there are numerical instabilities.
3113 */
3114 public static void estimate(
3115 final ECEFPositionAndVelocity positionAndVelocity, final Time propagationInterval,
3116 final INSLooselyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
3117 final Angle previousLatitude, final INSLooselyCoupledKalmanConfig config,
3118 final INSLooselyCoupledKalmanState result) throws AlgebraException {
3119 estimate(positionAndVelocity, propagationInterval, previousState, fx, fy, fz, convertAngle(previousLatitude),
3120 config, result);
3121 }
3122
3123 /**
3124 * Converts time instance into a value expressed in seconds.
3125 *
3126 * @param time time instance to be converted.
3127 * @return time value expressed in seconds.
3128 */
3129 private static double convertTime(final Time time) {
3130 return TimeConverter.convert(time.getValue().doubleValue(), time.getUnit(), TimeUnit.SECOND);
3131 }
3132
3133 /**
3134 * Converts angle instance into a value expressed in radians.
3135 *
3136 * @param angle angle instance to be converted.
3137 * @return angle value expressed in radians.
3138 */
3139 private static double convertAngle(final Angle angle) {
3140 return AngleConverter.convert(angle.getValue().doubleValue(), angle.getUnit(), AngleUnit.RADIANS);
3141 }
3142 }