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.navigation.frames.CoordinateTransformation;
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.GNSSMeasurement;
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 import java.util.Collection;
38
39 /**
40 * Implements one cycle of the tightly coupled INS/GNSS
41 * Kalman filter plus closed-loop correction of all inertial states.
42 * This implementation is based on the equations defined in "Principles of GNSS, Inertial, and Multisensor
43 * Integrated Navigation Systems, Second Edition" and on the companion software available at:
44 * <a href="https://github.com/ymjdz/MATLAB-Codes/blob/master/TC_KF_Epoch.m">
45 * https://github.com/ymjdz/MATLAB-Codes/blob/master/TC_KF_Epoch.m
46 * </a>
47 */
48 public class INSTightlyCoupledKalmanEpochEstimator {
49
50 /**
51 * Speed of light in the vacuum expressed in meters per second (m/s).
52 */
53 public static final double SPEED_OF_LIGHT = Constants.SPEED_OF_LIGHT;
54
55 /**
56 * Earth rotation rate expressed in radians per second (rad/s).
57 */
58 public static final double EARTH_ROTATION_RATE = Constants.EARTH_ROTATION_RATE;
59
60 /**
61 * The equatorial radius of WGS84 ellipsoid (6378137 m) defining Earth's shape.
62 */
63 public static final double EARTH_EQUATORIAL_RADIUS_WGS84 = Constants.EARTH_EQUATORIAL_RADIUS_WGS84;
64
65 /**
66 * Earth eccentricity as defined on the WGS84 ellipsoid.
67 */
68 public static final double EARTH_ECCENTRICITY = Constants.EARTH_ECCENTRICITY;
69
70 /**
71 * Constructor.
72 * Prevents instantiation of helper class.
73 */
74 private INSTightlyCoupledKalmanEpochEstimator() {
75 }
76
77 /**
78 * Estimates the update of Kalman filter state and covariance matrix for a single
79 * epoch.
80 *
81 * @param measurements satellite measurements data.
82 * @param propagationInterval propagation interval expressed in seconds (s).
83 * @param previousState previous Kalman filter state.
84 * @param fx measured specific force resolved along body frame
85 * x-axis and expressed in meters per squared
86 * second (m/s^2).
87 * @param fy measured specific force resolved along body frame
88 * y-axis and expressed in meters per squared
89 * second (m/s^2).
90 * @param fz measured specific force resolved along body frame
91 * z-axis and expressed in meters per squared
92 * second (m/s^2).
93 * @param previousLatitude previous latitude solution expressed in radians (rad).
94 * @param config Tightly Coupled Kalman filter configuration.
95 * @param result instance where new state of Kalman filter will be
96 * stored.
97 * @throws AlgebraException if there are numerical instabilities.
98 */
99 public static void estimate(
100 final Collection<GNSSMeasurement> measurements, final double propagationInterval,
101 final INSTightlyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
102 final double previousLatitude, final INSTightlyCoupledKalmanConfig config,
103 final INSTightlyCoupledKalmanState result) throws AlgebraException {
104
105 // Skew symmetric matrix of Earth rate
106 final var omegaIe = Utils.skewMatrix(new double[]{0.0, 0.0, EARTH_ROTATION_RATE});
107
108 // SYSTEM PROPAGATION PHASE
109
110 // 1. Determine transition matrix using (14.50) (first-order approx)
111 final var phiMatrix = Matrix.identity(
112 INSTightlyCoupledKalmanState.NUM_PARAMS, INSTightlyCoupledKalmanState.NUM_PARAMS);
113
114 final var tmp1 = omegaIe.multiplyByScalarAndReturnNew(propagationInterval);
115 final var tmp2 = phiMatrix.getSubmatrix(0, 0, 2, 2);
116 tmp2.subtract(tmp1);
117
118 phiMatrix.setSubmatrix(0, 0, 2, 2, tmp2);
119
120 final var estCbeOld = previousState.getBodyToEcefCoordinateTransformationMatrix();
121 tmp1.copyFrom(estCbeOld);
122 tmp1.multiplyByScalar(propagationInterval);
123
124 phiMatrix.setSubmatrix(0, 12, 2, 14, tmp1);
125 phiMatrix.setSubmatrix(3, 9, 5, 11, tmp1);
126
127 final var measFibb = new Matrix(BodyKinematics.COMPONENTS, 1);
128 measFibb.setElementAtIndex(0, fx);
129 measFibb.setElementAtIndex(1, fy);
130 measFibb.setElementAtIndex(2, fz);
131
132 estCbeOld.multiply(measFibb, tmp1);
133
134 Utils.skewMatrix(tmp1, tmp2);
135 tmp2.multiplyByScalar(-propagationInterval);
136
137 phiMatrix.setSubmatrix(3, 0, 5, 2, tmp2);
138
139 phiMatrix.getSubmatrix(3, 3, 5, 5, tmp1);
140 tmp2.copyFrom(omegaIe);
141 tmp2.multiplyByScalar(2.0 * propagationInterval);
142 tmp1.subtract(tmp2);
143 phiMatrix.setSubmatrix(3, 3, 5, 5, tmp1);
144
145 final var sinPrevLat = Math.sin(previousLatitude);
146 final var cosPrevLat = Math.cos(previousLatitude);
147 final var sinPrevLat2 = sinPrevLat * sinPrevLat;
148 final var cosPrevLat2 = cosPrevLat * cosPrevLat;
149
150 // From (2.137)
151 final var geocentricRadius = EARTH_EQUATORIAL_RADIUS_WGS84
152 / Math.sqrt(1.0 - Math.pow(EARTH_ECCENTRICITY * sinPrevLat, 2.0))
153 * Math.sqrt(cosPrevLat2 + Math.pow(1.0 - EARTH_ECCENTRICITY * EARTH_ECCENTRICITY, 2.0) * sinPrevLat2);
154
155 final var prevX = previousState.getX();
156 final var prevY = previousState.getY();
157 final var prevZ = previousState.getZ();
158 final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(prevX, prevY, prevZ);
159
160 final var previousPositionNorm = Math.sqrt(prevX * prevX + prevY * prevY + prevZ * prevZ);
161
162 final var estRebeOld = new Matrix(com.irurueta.navigation.frames.ECEFPosition.COMPONENTS, 1);
163 estRebeOld.setElementAtIndex(0, prevX);
164 estRebeOld.setElementAtIndex(1, prevY);
165 estRebeOld.setElementAtIndex(2, prevZ);
166
167 final var g = gravity.asMatrix();
168 g.multiplyByScalar(-2.0 * propagationInterval / geocentricRadius);
169
170 final var estRebeOldTrans = estRebeOld.transposeAndReturnNew();
171 estRebeOldTrans.multiplyByScalar(1.0 / previousPositionNorm);
172
173 g.multiply(estRebeOldTrans, tmp1);
174
175 phiMatrix.setSubmatrix(3, 6, 5, 8, tmp1);
176
177 for (var i = 0; i < ECEFPosition.COMPONENTS; i++) {
178 phiMatrix.setElementAt(6 + i, 3 + i, propagationInterval);
179 }
180
181 phiMatrix.setElementAt(15, 16, propagationInterval);
182
183 // 2. Determine approximate system noise covariance matrix using (14.82)
184 final var qPrimeMatrix = new Matrix(
185 INSTightlyCoupledKalmanState.NUM_PARAMS, INSTightlyCoupledKalmanState.NUM_PARAMS);
186
187 final var gyroNoisePSD = config.getGyroNoisePSD();
188 final var gyroNoiseValue = gyroNoisePSD * propagationInterval;
189 for (var i = 0; i < 3; i++) {
190 qPrimeMatrix.setElementAt(i, i, gyroNoiseValue);
191 }
192
193 final var accelNoisePSD = config.getAccelerometerNoisePSD();
194 final var accelNoiseValue = accelNoisePSD * propagationInterval;
195 for (var i = 3; i < 6; i++) {
196 qPrimeMatrix.setElementAt(i, i, accelNoiseValue);
197 }
198
199 final var accelBiasPSD = config.getAccelerometerBiasPSD();
200 final var accelBiasValue = accelBiasPSD * propagationInterval;
201 for (var i = 9; i < 12; i++) {
202 qPrimeMatrix.setElementAt(i, i, accelBiasValue);
203 }
204
205 final var gyroBiasPSD = config.getGyroBiasPSD();
206 final var gyroBiasValue = gyroBiasPSD * propagationInterval;
207 for (var i = 12; i < 15; i++) {
208 qPrimeMatrix.setElementAt(i, i, gyroBiasValue);
209 }
210
211 final var clockPhasePSD = config.getClockPhasePSD();
212 final var clockPhaseValue = clockPhasePSD * propagationInterval;
213 qPrimeMatrix.setElementAt(15, 15, clockPhaseValue);
214
215 final var clockFreqPSD = config.getClockFrequencyPSD();
216 final var clockFreqValue = clockFreqPSD * propagationInterval;
217 qPrimeMatrix.setElementAt(16, 16, clockFreqValue);
218
219 // 3. Propagate state estimates using (3.14) noting that only the clock
220 // states are non-zero due to closed-loop correction
221 final var prevClockOffset = previousState.getReceiverClockOffset();
222 final var prevClockDrift = previousState.getReceiverClockDrift();
223
224 final var xEstPropagated = new Matrix(INSTightlyCoupledKalmanState.NUM_PARAMS, 1);
225 xEstPropagated.setElementAtIndex(15, prevClockOffset + prevClockDrift * propagationInterval);
226 xEstPropagated.setElementAtIndex(16, prevClockDrift);
227
228 // 4. Propagate state estimation error covariance matrix using (3.46)
229 final var pMatrixOld = previousState.getCovariance();
230
231 qPrimeMatrix.multiplyByScalar(0.5);
232
233 final var tmp3 = pMatrixOld.addAndReturnNew(qPrimeMatrix);
234 final var pMatrixPropagated = phiMatrix.multiplyAndReturnNew(tmp3);
235
236 phiMatrix.transpose();
237 pMatrixPropagated.multiply(phiMatrix);
238
239 pMatrixPropagated.add(qPrimeMatrix);
240
241 // MEASUREMENT UPDATE PHASE
242
243 final var numberOfMeasurements = measurements.size();
244 final var uAseT = new Matrix(numberOfMeasurements, 3);
245 final var predMeas = new Matrix(numberOfMeasurements, 2);
246
247 final var cei = Matrix.identity(CoordinateTransformation.ROWS, CoordinateTransformation.COLS);
248 final var satellitePosition = new Matrix(CoordinateTransformation.ROWS, 1);
249 final var satelliteVelocity = new Matrix(CoordinateTransformation.ROWS, 1);
250 final var deltaR = new Matrix(CoordinateTransformation.ROWS, 1);
251 final var tmp1b = new Matrix(CoordinateTransformation.ROWS, 1);
252 final var tmp2b = new Matrix(CoordinateTransformation.ROWS, 1);
253 final var tmp3b = new Matrix(CoordinateTransformation.ROWS, 1);
254 final var tmp4b = new Matrix(CoordinateTransformation.ROWS, 1);
255 final var tmp5b = new Matrix(CoordinateTransformation.ROWS, 1);
256 final var tmp6b = new Matrix(CoordinateTransformation.ROWS, 1);
257 final var tmp7b = new Matrix(1, CoordinateTransformation.ROWS);
258
259 final var prevVx = previousState.getVx();
260 final var prevVy = previousState.getVy();
261 final var prevVz = previousState.getVz();
262
263 final var estVebeOld = new Matrix(ECEFVelocity.COMPONENTS, 1);
264 estVebeOld.setElementAtIndex(0, prevVx);
265 estVebeOld.setElementAtIndex(1, prevVy);
266 estVebeOld.setElementAtIndex(2, prevVz);
267
268 var j = 0;
269 for (final var measurement : measurements) {
270 // Predict approx range
271 final var measX = measurement.getX();
272 final var measY = measurement.getY();
273 final var measZ = measurement.getZ();
274
275 final var deltaX = measX - prevX;
276 final var deltaY = measY - prevY;
277 final var deltaZ = measZ - prevZ;
278 final var approxRange = Math.sqrt(deltaX * deltaX + deltaY * deltaY + deltaZ * deltaZ);
279
280 // Calculate frame rotation during signal transit time using (8.36)
281 final var ceiValue = EARTH_ROTATION_RATE * approxRange / SPEED_OF_LIGHT;
282 cei.setElementAt(0, 1, ceiValue);
283 cei.setElementAt(1, 0, -ceiValue);
284
285 // Predict pseudo-range using (9.165)
286 satellitePosition.setElementAtIndex(0, measX);
287 satellitePosition.setElementAtIndex(1, measY);
288 satellitePosition.setElementAtIndex(2, measZ);
289
290 cei.multiply(satellitePosition, deltaR);
291 for (var i = 0; i < CoordinateTransformation.ROWS; i++) {
292 deltaR.setElementAtIndex(i, deltaR.getElementAtIndex(i) - estRebeOld.getElementAtIndex(i));
293 }
294 final var range = Utils.normF(deltaR);
295
296 predMeas.setElementAt(j, 0, range + xEstPropagated.getElementAtIndex(15));
297
298 // Predict line of sight
299 for (var i = 0; i < CoordinateTransformation.ROWS; i++) {
300 uAseT.setElementAt(j, i, deltaR.getElementAtIndex(i) / range);
301 }
302
303 // Predict pseudo-range rae using (9.165)
304 satelliteVelocity.setElementAtIndex(0, measurement.getVx());
305 satelliteVelocity.setElementAtIndex(1, measurement.getVy());
306 satelliteVelocity.setElementAtIndex(2, measurement.getVz());
307
308 omegaIe.multiply(satellitePosition, tmp1b);
309 satelliteVelocity.add(tmp1b, tmp2b);
310 cei.multiply(tmp2b, tmp3b);
311
312 omegaIe.multiply(estRebeOld, tmp4b);
313 estVebeOld.add(tmp4b, tmp6b);
314
315 tmp3b.subtract(tmp6b, tmp5b);
316
317 uAseT.getSubmatrix(j, 0, j, 2, tmp7b);
318
319 final var rangeRate = Utils.dotProduct(tmp7b, tmp5b);
320
321 predMeas.setElementAt(j, 1, rangeRate + xEstPropagated.getElementAtIndex(16));
322
323 j++;
324 }
325
326 // 5. Set-up measurement matrix using (14.126)
327 final var h = new Matrix(2 * numberOfMeasurements, INSTightlyCoupledKalmanState.NUM_PARAMS);
328 for (int j1 = 0, j2 = numberOfMeasurements; j1 < numberOfMeasurements; j1++, j2++) {
329 for (int i1 = 0, i2 = 6, i3 = 3; i1 < CoordinateTransformation.ROWS; i1++, i2++, i3++) {
330 final var value = uAseT.getElementAt(j1, i1);
331
332 h.setElementAt(j1, i2, value);
333 h.setElementAt(j2, i3, value);
334 }
335 h.setElementAt(j1, 15, 1.0);
336 h.setElementAt(j2, 16, 1.0);
337 }
338
339 // 6. Set-up measurement noise covariance matrix assuming all measurements are independent
340 // and have equal variance for a given measurement type.
341 final var pseudoRangeSD = config.getPseudoRangeSD();
342 final var pseudoRangeSD2 = pseudoRangeSD * pseudoRangeSD;
343 final var rangeRateSD = config.getRangeRateSD();
344 final var rangeRateSD2 = rangeRateSD * rangeRateSD;
345 final var r = new Matrix(2 * numberOfMeasurements, 2 * numberOfMeasurements);
346 for (int i1 = 0, i2 = numberOfMeasurements; i1 < numberOfMeasurements; i1++, i2++) {
347 r.setElementAt(i1, i1, pseudoRangeSD2);
348 r.setElementAt(i2, i2, rangeRateSD2);
349 }
350
351 // 7. Calculate Kalman gain using (3.21)
352 final var hTransposed = h.transposeAndReturnNew();
353 final var tmp8b = h.multiplyAndReturnNew(pMatrixPropagated.multiplyAndReturnNew(hTransposed));
354 tmp8b.add(r);
355 final var tmp9b = Utils.inverse(tmp8b);
356 final var k = pMatrixPropagated.multiplyAndReturnNew(hTransposed);
357 k.multiply(tmp9b);
358
359 // 8. Formulate measurement innovations using (14.119)
360 final var deltaZ = new Matrix(2 * numberOfMeasurements, 1);
361 var i1 = 0;
362 var i2 = numberOfMeasurements;
363 for (final var measurement : measurements) {
364 deltaZ.setElementAtIndex(i1, measurement.getPseudoRange() - predMeas.getElementAt(i1, 0));
365 deltaZ.setElementAtIndex(i2, measurement.getPseudoRate() - predMeas.getElementAt(i1, 1));
366
367 i1++;
368 i2++;
369 }
370
371 // 9. Update state estimates using (3.24)
372 xEstPropagated.add(k.multiplyAndReturnNew(deltaZ));
373
374 // xEstPropagated now contains updated state
375
376 // 10. Update state estimation error covariance matrix using (3.25)
377 Matrix updatedCovariance = result.getCovariance();
378 if (updatedCovariance == null || updatedCovariance.getRows() != INSTightlyCoupledKalmanState.NUM_PARAMS
379 || updatedCovariance.getColumns() != INSTightlyCoupledKalmanState.NUM_PARAMS) {
380 updatedCovariance = Matrix.identity(
381 INSTightlyCoupledKalmanState.NUM_PARAMS, INSTightlyCoupledKalmanState.NUM_PARAMS);
382 } else {
383 Matrix.identity(updatedCovariance);
384 }
385 k.multiply(h);
386 updatedCovariance.subtract(k);
387 updatedCovariance.multiply(pMatrixPropagated);
388
389 // CLOSED-LOOP CORRECTION
390
391 // Correct attitude, velocity, and position using (14.7-9)
392
393 final var estCbeNew = Matrix.identity(CoordinateTransformation.ROWS, CoordinateTransformation.COLS);
394
395 xEstPropagated.getSubmatrix(0, 0, 2, 0, tmp1b);
396 estCbeNew.subtract(Utils.skewMatrix(tmp1b));
397 estCbeNew.multiply(estCbeOld);
398
399
400 result.setBodyToEcefCoordinateTransformationMatrix(estCbeNew);
401 result.setVelocityCoordinates(
402 prevVx - xEstPropagated.getElementAtIndex(3),
403 prevVy - xEstPropagated.getElementAtIndex(4),
404 prevVz - xEstPropagated.getElementAtIndex(5));
405 result.setPositionCoordinates(
406 prevX - xEstPropagated.getElementAtIndex(6),
407 prevY - xEstPropagated.getElementAtIndex(7),
408 prevZ - xEstPropagated.getElementAtIndex(8));
409 result.setCovariance(updatedCovariance);
410
411 // Update IMU bias and GNSS receiver clock estimates
412 final var prevAccelBiasX = previousState.getAccelerationBiasX();
413 final var prevAccelBiasY = previousState.getAccelerationBiasY();
414 final var prevAccelBiasZ = previousState.getAccelerationBiasZ();
415 final var prevGyroBiasX = previousState.getGyroBiasX();
416 final var prevGyroBiasY = previousState.getGyroBiasY();
417 final var prevGyroBiasZ = previousState.getGyroBiasZ();
418
419 result.setAccelerationBiasCoordinates(
420 prevAccelBiasX + xEstPropagated.getElementAtIndex(9),
421 prevAccelBiasY + xEstPropagated.getElementAtIndex(10),
422 prevAccelBiasZ + xEstPropagated.getElementAtIndex(11));
423 result.setGyroBiasCoordinates(
424 prevGyroBiasX + xEstPropagated.getElementAtIndex(12),
425 prevGyroBiasY + xEstPropagated.getElementAtIndex(13),
426 prevGyroBiasZ + xEstPropagated.getElementAtIndex(14));
427
428 result.setReceiverClockOffset(xEstPropagated.getElementAtIndex(15));
429 result.setReceiverClockDrift(xEstPropagated.getElementAtIndex(16));
430 }
431
432 /**
433 * Estimates the update of Kalman filter state and covariance matrix for a single
434 * epoch.
435 *
436 * @param measurements satellite measurements data.
437 * @param propagationInterval propagation interval expressed in seconds (s).
438 * @param previousState previous Kalman filter state.
439 * @param fx measured specific force resolved along body frame
440 * x-axis and expressed in meters per squared
441 * second (m/s^2).
442 * @param fy measured specific force resolved along body frame
443 * y-axis and expressed in meters per squared
444 * second (m/s^2).
445 * @param fz measured specific force resolved along body frame
446 * z-axis and expressed in meters per squared
447 * second (m/s^2).
448 * @param previousLatitude previous latitude solution expressed in radians (rad).
449 * @param config Tightly Coupled Kalman filter configuration.
450 * @return new state of Kalman filter.
451 * @throws AlgebraException if there are numerical instabilities.
452 */
453 public static INSTightlyCoupledKalmanState estimate(
454 final Collection<GNSSMeasurement> measurements, final double propagationInterval,
455 final INSTightlyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
456 final double previousLatitude, final INSTightlyCoupledKalmanConfig config) throws AlgebraException {
457 final var result = new INSTightlyCoupledKalmanState();
458 estimate(measurements, propagationInterval, previousState, fx, fy, fz, previousLatitude, config, result);
459 return result;
460 }
461
462 /**
463 * Estimates the update of Kalman filter state and covariance matrix for a single
464 * epoch.
465 *
466 * @param measurements satellite measurements data.
467 * @param propagationInterval propagation interval.
468 * @param previousState previous Kalman filter state.
469 * @param fx measured specific force resolved along body frame
470 * x-axis and expressed in meters per squared
471 * second (m/s^2).
472 * @param fy measured specific force resolved along body frame
473 * y-axis and expressed in meters per squared
474 * second (m/s^2).
475 * @param fz measured specific force resolved along body frame
476 * z-axis and expressed in meters per squared
477 * second (m/s^2).
478 * @param previousLatitude previous latitude solution expressed in radians (rad).
479 * @param config Tightly Coupled Kalman filter configuration.
480 * @param result instance where new state of Kalman filter will be
481 * stored.
482 * @throws AlgebraException if there are numerical instabilities.
483 */
484 public static void estimate(
485 final Collection<GNSSMeasurement> measurements, final Time propagationInterval,
486 final INSTightlyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
487 final double previousLatitude, final INSTightlyCoupledKalmanConfig config,
488 final INSTightlyCoupledKalmanState result) throws AlgebraException {
489 estimate(measurements, convertTime(propagationInterval), previousState, fx, fy, fz, previousLatitude, config,
490 result);
491 }
492
493 /**
494 * Estimates the update of Kalman filter state and covariance matrix for a single
495 * epoch.
496 *
497 * @param measurements satellite measurements data.
498 * @param propagationInterval propagation interval.
499 * @param previousState previous Kalman filter state.
500 * @param fx measured specific force resolved along body frame
501 * x-axis and expressed in meters per squared
502 * second (m/s^2).
503 * @param fy measured specific force resolved along body frame
504 * y-axis and expressed in meters per squared
505 * second (m/s^2).
506 * @param fz measured specific force resolved along body frame
507 * z-axis and expressed in meters per squared
508 * second (m/s^2).
509 * @param previousLatitude previous latitude solution expressed in radians (rad).
510 * @param config Tightly Coupled Kalman filter configuration.
511 * @return new state of Kalman filter.
512 * @throws AlgebraException if there are numerical instabilities.
513 */
514 public static INSTightlyCoupledKalmanState estimate(
515 final Collection<GNSSMeasurement> measurements, final Time propagationInterval,
516 final INSTightlyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
517 final double previousLatitude, final INSTightlyCoupledKalmanConfig config) throws AlgebraException {
518 return estimate(measurements, convertTime(propagationInterval), previousState, fx, fy, fz, previousLatitude,
519 config);
520 }
521
522 /**
523 * Estimates the update of Kalman filter state and covariance matrix for a single
524 * epoch.
525 *
526 * @param measurements satellite measurements data.
527 * @param propagationInterval propagation interval expressed in seconds (s).
528 * @param previousState previous Kalman filter state.
529 * @param fx measured specific force resolved along body frame
530 * x-axis and expressed in meters per squared
531 * second (m/s^2).
532 * @param fy measured specific force resolved along body frame
533 * y-axis and expressed in meters per squared
534 * second (m/s^2).
535 * @param fz measured specific force resolved along body frame
536 * z-axis and expressed in meters per squared
537 * second (m/s^2).
538 * @param config Tightly Coupled Kalman filter configuration.
539 * @param result instance where new state of Kalman filter will be
540 * stored.
541 * @throws AlgebraException if there are numerical instabilities.
542 */
543 public static void estimate(
544 final Collection<GNSSMeasurement> measurements, final double propagationInterval,
545 final INSTightlyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
546 final INSTightlyCoupledKalmanConfig config, final INSTightlyCoupledKalmanState result) throws AlgebraException {
547
548 final var prevNedPosition = new NEDPosition();
549 final var prevNedVelocity = new NEDVelocity();
550 ECEFtoNEDPositionVelocityConverter.convertECEFtoNED(
551 previousState.getX(), previousState.getY(), previousState.getZ(),
552 previousState.getVx(), previousState.getVy(), previousState.getVz(), prevNedPosition, prevNedVelocity);
553
554 final var previousLatitude = prevNedPosition.getLatitude();
555
556 estimate(measurements, propagationInterval, previousState, fx, fy, fz, previousLatitude, config, result);
557 }
558
559 /**
560 * Estimates the update of Kalman filter state and covariance matrix for a single
561 * epoch.
562 *
563 * @param measurements satellite measurements data.
564 * @param propagationInterval propagation interval expressed in seconds (s).
565 * @param previousState previous Kalman filter state.
566 * @param fx measured specific force resolved along body frame
567 * x-axis and expressed in meters per squared
568 * second (m/s^2).
569 * @param fy measured specific force resolved along body frame
570 * y-axis and expressed in meters per squared
571 * second (m/s^2).
572 * @param fz measured specific force resolved along body frame
573 * z-axis and expressed in meters per squared
574 * second (m/s^2).
575 * @param config Tightly Coupled Kalman filter configuration.
576 * @return new state of Kalman filter.
577 * @throws AlgebraException if there are numerical instabilities.
578 */
579 public static INSTightlyCoupledKalmanState estimate(
580 final Collection<GNSSMeasurement> measurements, final double propagationInterval,
581 final INSTightlyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
582 final INSTightlyCoupledKalmanConfig config) throws AlgebraException {
583 final var result = new INSTightlyCoupledKalmanState();
584 estimate(measurements, propagationInterval, previousState, fx, fy, fz, config, result);
585 return result;
586 }
587
588 /**
589 * Estimates the update of Kalman filter state and covariance matrix for a single
590 * epoch.
591 *
592 * @param measurements satellite measurements data.
593 * @param propagationInterval propagation interval.
594 * @param previousState previous Kalman filter state.
595 * @param fx measured specific force resolved along body frame
596 * x-axis and expressed in meters per squared
597 * second (m/s^2).
598 * @param fy measured specific force resolved along body frame
599 * y-axis and expressed in meters per squared
600 * second (m/s^2).
601 * @param fz measured specific force resolved along body frame
602 * z-axis and expressed in meters per squared
603 * second (m/s^2).
604 * @param config Tightly Coupled Kalman filter configuration.
605 * @param result instance where new state of Kalman filter will be
606 * stored.
607 * @throws AlgebraException if there are numerical instabilities.
608 */
609 public static void estimate(
610 final Collection<GNSSMeasurement> measurements, final Time propagationInterval,
611 final INSTightlyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
612 final INSTightlyCoupledKalmanConfig config, final INSTightlyCoupledKalmanState result)
613 throws AlgebraException {
614 estimate(measurements, convertTime(propagationInterval), previousState, fx, fy, fz, config, result);
615 }
616
617 /**
618 * Estimates the update of Kalman filter state and covariance matrix for a single
619 * epoch.
620 *
621 * @param measurements satellite measurements data.
622 * @param propagationInterval propagation interval.
623 * @param previousState previous Kalman filter state.
624 * @param fx measured specific force resolved along body frame
625 * x-axis and expressed in meters per squared
626 * second (m/s^2).
627 * @param fy measured specific force resolved along body frame
628 * y-axis and expressed in meters per squared
629 * second (m/s^2).
630 * @param fz measured specific force resolved along body frame
631 * second (m/s^2).
632 * @param config Tightly Coupled Kalman filter configuration.
633 * @return new state of Kalman filter.
634 * @throws AlgebraException if there are numerical instabilities.
635 */
636 public static INSTightlyCoupledKalmanState estimate(
637 final Collection<GNSSMeasurement> measurements, final Time propagationInterval,
638 final INSTightlyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
639 final INSTightlyCoupledKalmanConfig config) throws AlgebraException {
640 return estimate(measurements, convertTime(propagationInterval), previousState, fx, fy, fz, config);
641 }
642
643 /**
644 * Estimates the update of Kalman filter state and covariance matrix for a single
645 * epoch.
646 *
647 * @param measurements satellite measurements data.
648 * @param propagationInterval propagation interval expressed in seconds (s).
649 * @param previousState previous Kalman filter state.
650 * @param bodyKinematics body kinematics containing measured specific force
651 * resolved along body frame axes.
652 * @param previousLatitude previous latitude solution expressed in radians (rad).
653 * @param config Tightly Coupled Kalman filter configuration.
654 * @param result instance where new state of Kalman filter will be
655 * stored.
656 * @throws AlgebraException if there are numerical instabilities.
657 */
658 public static void estimate(
659 final Collection<GNSSMeasurement> measurements, final double propagationInterval,
660 final INSTightlyCoupledKalmanState previousState, final BodyKinematics bodyKinematics,
661 final double previousLatitude, final INSTightlyCoupledKalmanConfig config,
662 final INSTightlyCoupledKalmanState result) throws AlgebraException {
663
664 final var fx = bodyKinematics.getFx();
665 final var fy = bodyKinematics.getFy();
666 final var fz = bodyKinematics.getFz();
667
668 estimate(measurements, propagationInterval, previousState, fx, fy, fz, previousLatitude, config, result);
669 }
670
671 /**
672 * Estimates the update of Kalman filter state and covariance matrix for a single
673 * epoch.
674 *
675 * @param measurements satellite measurements data.
676 * @param propagationInterval propagation interval expressed in seconds (s).
677 * @param previousState previous Kalman filter state.
678 * @param bodyKinematics body kinematics containing measured specific force
679 * resolved along body frame axes.
680 * @param previousLatitude previous latitude solution expressed in radians (rad).
681 * @param config Tightly Coupled Kalman filter configuration.
682 * @return new state of Kalman filter.
683 * @throws AlgebraException if there are numerical instabilities.
684 */
685 public static INSTightlyCoupledKalmanState estimate(
686 final Collection<GNSSMeasurement> measurements, final double propagationInterval,
687 final INSTightlyCoupledKalmanState previousState, final BodyKinematics bodyKinematics,
688 final double previousLatitude, final INSTightlyCoupledKalmanConfig config) throws AlgebraException {
689 final var result = new INSTightlyCoupledKalmanState();
690 estimate(measurements, propagationInterval, previousState, bodyKinematics, previousLatitude, config, result);
691 return result;
692 }
693
694 /**
695 * Estimates the update of Kalman filter state and covariance matrix for a single
696 * epoch.
697 *
698 * @param measurements satellite measurements data.
699 * @param propagationInterval propagation interval.
700 * @param previousState previous Kalman filter state.
701 * @param bodyKinematics body kinematics containing measured specific force
702 * resolved along body frame axes.
703 * @param previousLatitude previous latitude solution expressed in radians (rad).
704 * @param config Tightly Coupled Kalman filter configuration.
705 * @param result instance where new state of Kalman filter will be
706 * stored.
707 * @throws AlgebraException if there are numerical instabilities.
708 */
709 public static void estimate(
710 final Collection<GNSSMeasurement> measurements, final Time propagationInterval,
711 final INSTightlyCoupledKalmanState previousState, final BodyKinematics bodyKinematics,
712 final double previousLatitude, final INSTightlyCoupledKalmanConfig config,
713 final INSTightlyCoupledKalmanState result) throws AlgebraException {
714 estimate(measurements, convertTime(propagationInterval), previousState, bodyKinematics, previousLatitude,
715 config, result);
716 }
717
718 /**
719 * Estimates the update of Kalman filter state and covariance matrix for a single
720 * epoch.
721 *
722 * @param measurements satellite measurements data.
723 * @param propagationInterval propagation interval.
724 * @param previousState previous Kalman filter state.
725 * @param bodyKinematics body kinematics containing measured specific force
726 * resolved along body frame axes.
727 * @param previousLatitude previous latitude solution expressed in radians (rad).
728 * @param config Tightly Coupled Kalman filter configuration.
729 * @return new state of Kalman filter.
730 * @throws AlgebraException if there are numerical instabilities.
731 */
732 public static INSTightlyCoupledKalmanState estimate(
733 final Collection<GNSSMeasurement> measurements, final Time propagationInterval,
734 final INSTightlyCoupledKalmanState previousState, final BodyKinematics bodyKinematics,
735 final double previousLatitude, final INSTightlyCoupledKalmanConfig config) throws AlgebraException {
736 return estimate(measurements, convertTime(propagationInterval), previousState, bodyKinematics, previousLatitude,
737 config);
738 }
739
740 /**
741 * Estimates the update of Kalman filter state and covariance matrix for a single
742 * epoch.
743 *
744 * @param measurements satellite measurements data.
745 * @param propagationInterval propagation interval expressed in seconds (s).
746 * @param previousState previous Kalman filter state.
747 * @param bodyKinematics body kinematics containing measured specific force
748 * resolved along body frame axes.
749 * @param config Tightly Coupled Kalman filter configuration.
750 * @param result instance where new state of Kalman filter will be
751 * stored.
752 * @throws AlgebraException if there are numerical instabilities.
753 */
754 public static void estimate(
755 final Collection<GNSSMeasurement> measurements, final double propagationInterval,
756 final INSTightlyCoupledKalmanState previousState, final BodyKinematics bodyKinematics,
757 final INSTightlyCoupledKalmanConfig config, final INSTightlyCoupledKalmanState result)
758 throws AlgebraException {
759
760 final var fx = bodyKinematics.getFx();
761 final var fy = bodyKinematics.getFy();
762 final var fz = bodyKinematics.getFz();
763
764 estimate(measurements, propagationInterval, previousState, fx, fy, fz, config, result);
765 }
766
767 /**
768 * Estimates the update of Kalman filter state and covariance matrix for a single
769 * epoch.
770 *
771 * @param measurements satellite measurements data.
772 * @param propagationInterval propagation interval expressed in seconds (s).
773 * @param previousState previous Kalman filter state.
774 * @param bodyKinematics body kinematics containing measured specific force
775 * resolved along body frame axes.
776 * @param config Tightly Coupled Kalman filter configuration.
777 * @return new state of Kalman filter.
778 * @throws AlgebraException if there are numerical instabilities.
779 */
780 public static INSTightlyCoupledKalmanState estimate(
781 final Collection<GNSSMeasurement> measurements, final double propagationInterval,
782 final INSTightlyCoupledKalmanState previousState, final BodyKinematics bodyKinematics,
783 final INSTightlyCoupledKalmanConfig config) throws AlgebraException {
784 final var result = new INSTightlyCoupledKalmanState();
785 estimate(measurements, propagationInterval, previousState, bodyKinematics, config, result);
786 return result;
787 }
788
789 /**
790 * Estimates the update of Kalman filter state and covariance matrix for a single
791 * epoch.
792 *
793 * @param measurements satellite measurements data.
794 * @param propagationInterval propagation interval.
795 * @param previousState previous Kalman filter state.
796 * @param bodyKinematics body kinematics containing measured specific force
797 * resolved along body frame axes.
798 * @param config Tightly Coupled Kalman filter configuration.
799 * @param result instance where new state of Kalman filter will be
800 * stored.
801 * @throws AlgebraException if there are numerical instabilities.
802 */
803 public static void estimate(
804 final Collection<GNSSMeasurement> measurements, final Time propagationInterval,
805 final INSTightlyCoupledKalmanState previousState, final BodyKinematics bodyKinematics,
806 final INSTightlyCoupledKalmanConfig config, final INSTightlyCoupledKalmanState result)
807 throws AlgebraException {
808 estimate(measurements, convertTime(propagationInterval), previousState, bodyKinematics, config, result);
809 }
810
811 /**
812 * Estimates the update of Kalman filter state and covariance matrix for a single
813 * epoch.
814 *
815 * @param measurements satellite measurements data.
816 * @param propagationInterval propagation interval expressed in seconds (s).
817 * @param previousState previous Kalman filter state.
818 * @param bodyKinematics body kinematics containing measured specific force
819 * resolved along body frame axes.
820 * @param config Tightly Coupled Kalman filter configuration.
821 * @return new state of Kalman filter.
822 * @throws AlgebraException if there are numerical instabilities.
823 */
824 public static INSTightlyCoupledKalmanState estimate(
825 final Collection<GNSSMeasurement> measurements, final Time propagationInterval,
826 final INSTightlyCoupledKalmanState previousState, final BodyKinematics bodyKinematics,
827 final INSTightlyCoupledKalmanConfig config) throws AlgebraException {
828 return estimate(measurements, convertTime(propagationInterval), previousState, bodyKinematics, config);
829 }
830
831 /**
832 * Estimates the update of Kalman filter state and covariance matrix for a single
833 * epoch.
834 *
835 * @param measurements satellite measurements data.
836 * @param propagationInterval propagation interval expressed in seconds (s).
837 * @param previousState previous Kalman filter state.
838 * @param fx measured specific force resolved along body frame
839 * x-axis and expressed in meters per squared
840 * second (m/s^2).
841 * @param fy measured specific force resolved along body frame
842 * y-axis and expressed in meters per squared
843 * second (m/s^2).
844 * @param fz measured specific force resolved along body frame
845 * z-axis and expressed in meters per squared
846 * second (m/s^2).
847 * @param previousLatitude previous latitude solution.
848 * @param config Tightly Coupled Kalman filter configuration.
849 * @param result instance where new state of Kalman filter will be
850 * stored.
851 * @throws AlgebraException if there are numerical instabilities.
852 */
853 public static void estimate(
854 final Collection<GNSSMeasurement> measurements, final double propagationInterval,
855 final INSTightlyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
856 final Angle previousLatitude, final INSTightlyCoupledKalmanConfig config,
857 final INSTightlyCoupledKalmanState result) throws AlgebraException {
858 estimate(measurements, propagationInterval, previousState, fx, fy, fz, convertAngle(previousLatitude), config,
859 result);
860 }
861
862 /**
863 * Estimates the update of Kalman filter state and covariance matrix for a single
864 * epoch.
865 *
866 * @param measurements satellite measurements data.
867 * @param propagationInterval propagation interval expressed in seconds (s).
868 * @param previousState previous Kalman filter state.
869 * @param fx measured specific force resolved along body frame
870 * x-axis and expressed in meters per squared
871 * second (m/s^2).
872 * @param fy measured specific force resolved along body frame
873 * y-axis and expressed in meters per squared
874 * second (m/s^2).
875 * @param fz measured specific force resolved along body frame
876 * z-axis and expressed in meters per squared
877 * second (m/s^2).
878 * @param previousLatitude previous latitude solution.
879 * @param config Tightly Coupled Kalman filter configuration.
880 * @return new state of Kalman filter.
881 * @throws AlgebraException if there are numerical instabilities.
882 */
883 public static INSTightlyCoupledKalmanState estimate(
884 final Collection<GNSSMeasurement> measurements, final double propagationInterval,
885 final INSTightlyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
886 final Angle previousLatitude, final INSTightlyCoupledKalmanConfig config) throws AlgebraException {
887 return estimate(measurements, propagationInterval, previousState, fx, fy, fz, convertAngle(previousLatitude),
888 config);
889 }
890
891 /**
892 * Estimates the update of Kalman filter state and covariance matrix for a single
893 * epoch.
894 *
895 * @param measurements satellite measurements data.
896 * @param propagationInterval propagation interval expressed in seconds (s).
897 * @param previousState previous Kalman filter state.
898 * @param fx measured specific force resolved along body frame
899 * x-axis and expressed in meters per squared
900 * second (m/s^2).
901 * @param fy measured specific force resolved along body frame
902 * y-axis and expressed in meters per squared
903 * second (m/s^2).
904 * @param fz measured specific force resolved along body frame
905 * z-axis and expressed in meters per squared
906 * second (m/s^2).
907 * @param previousLatitude previous latitude solution.
908 * @param config Tightly Coupled Kalman filter configuration.
909 * @param result instance where new state of Kalman filter will be
910 * stored.
911 * @throws AlgebraException if there are numerical instabilities.
912 */
913 public static void estimate(
914 final Collection<GNSSMeasurement> measurements, final Time propagationInterval,
915 final INSTightlyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
916 final Angle previousLatitude, final INSTightlyCoupledKalmanConfig config,
917 final INSTightlyCoupledKalmanState result) throws AlgebraException {
918 estimate(measurements, propagationInterval, previousState, fx, fy, fz, convertAngle(previousLatitude), config,
919 result);
920 }
921
922 /**
923 * Estimates the update of Kalman filter state and covariance matrix for a single
924 * epoch.
925 *
926 * @param measurements satellite measurements data.
927 * @param propagationInterval propagation interval expressed in seconds (s).
928 * @param previousState previous Kalman filter state.
929 * @param fx measured specific force resolved along body frame
930 * x-axis and expressed in meters per squared
931 * second (m/s^2).
932 * @param fy measured specific force resolved along body frame
933 * y-axis and expressed in meters per squared
934 * second (m/s^2).
935 * @param fz measured specific force resolved along body frame
936 * z-axis and expressed in meters per squared
937 * second (m/s^2).
938 * @param previousLatitude previous latitude solution.
939 * @param config Tightly Coupled Kalman filter configuration.
940 * @return new state of Kalman filter.
941 * @throws AlgebraException if there are numerical instabilities.
942 */
943 public static INSTightlyCoupledKalmanState estimate(
944 final Collection<GNSSMeasurement> measurements, final Time propagationInterval,
945 final INSTightlyCoupledKalmanState previousState, final double fx, final double fy, final double fz,
946 final Angle previousLatitude, final INSTightlyCoupledKalmanConfig config) throws AlgebraException {
947 return estimate(measurements, propagationInterval, previousState, fx, fy, fz, convertAngle(previousLatitude),
948 config);
949 }
950
951 /**
952 * Estimates the update of Kalman filter state and covariance matrix for a single
953 * epoch.
954 *
955 * @param measurements satellite measurements data.
956 * @param propagationInterval propagation interval expressed in seconds (s).
957 * @param previousState previous Kalman filter state.
958 * @param bodyKinematics body kinematics containing measured specific force
959 * resolved along body frame axes.
960 * @param previousLatitude previous latitude solution.
961 * @param config Tightly Coupled Kalman filter configuration.
962 * @param result instance where new state of Kalman filter will be
963 * stored.
964 * @throws AlgebraException if there are numerical instabilities.
965 */
966 public static void estimate(
967 final Collection<GNSSMeasurement> measurements, final double propagationInterval,
968 final INSTightlyCoupledKalmanState previousState, final BodyKinematics bodyKinematics,
969 final Angle previousLatitude, final INSTightlyCoupledKalmanConfig config,
970 final INSTightlyCoupledKalmanState result) throws AlgebraException {
971 estimate(measurements, propagationInterval, previousState, bodyKinematics, convertAngle(previousLatitude),
972 config, result);
973 }
974
975 /**
976 * Estimates the update of Kalman filter state and covariance matrix for a single
977 * epoch.
978 *
979 * @param measurements satellite measurements data.
980 * @param propagationInterval propagation interval expressed in seconds (s).
981 * @param previousState previous Kalman filter state.
982 * @param bodyKinematics body kinematics containing measured specific force
983 * resolved along body frame axes.
984 * @param previousLatitude previous latitude solution.
985 * @param config Tightly Coupled Kalman filter configuration.
986 * @return new state of Kalman filter.
987 * @throws AlgebraException if there are numerical instabilities.
988 */
989 public static INSTightlyCoupledKalmanState estimate(
990 final Collection<GNSSMeasurement> measurements, final double propagationInterval,
991 final INSTightlyCoupledKalmanState previousState, final BodyKinematics bodyKinematics,
992 final Angle previousLatitude, final INSTightlyCoupledKalmanConfig config) throws AlgebraException {
993 return estimate(measurements, propagationInterval, previousState, bodyKinematics,
994 convertAngle(previousLatitude), config);
995 }
996
997 /**
998 * Estimates the update of Kalman filter state and covariance matrix for a single
999 * epoch.
1000 *
1001 * @param measurements satellite measurements data.
1002 * @param propagationInterval propagation interval.
1003 * @param previousState previous Kalman filter state.
1004 * @param bodyKinematics body kinematics containing measured specific force
1005 * resolved along body frame axes.
1006 * @param previousLatitude previous latitude solution.
1007 * @param config Tightly Coupled Kalman filter configuration.
1008 * @param result instance where new state of Kalman filter will be
1009 * stored.
1010 * @throws AlgebraException if there are numerical instabilities.
1011 */
1012 public static void estimate(
1013 final Collection<GNSSMeasurement> measurements, final Time propagationInterval,
1014 final INSTightlyCoupledKalmanState previousState, final BodyKinematics bodyKinematics,
1015 final Angle previousLatitude, final INSTightlyCoupledKalmanConfig config,
1016 final INSTightlyCoupledKalmanState result) throws AlgebraException {
1017 estimate(measurements, propagationInterval, previousState, bodyKinematics, convertAngle(previousLatitude),
1018 config, result);
1019 }
1020
1021 /**
1022 * Estimates the update of Kalman filter state and covariance matrix for a single
1023 * epoch.
1024 *
1025 * @param measurements satellite measurements data.
1026 * @param propagationInterval propagation interval.
1027 * @param previousState previous Kalman filter state.
1028 * @param bodyKinematics body kinematics containing measured specific force
1029 * resolved along body frame axes.
1030 * @param previousLatitude previous latitude solution.
1031 * @param config Tightly Coupled Kalman filter configuration.
1032 * @return new state of Kalman filter.
1033 * @throws AlgebraException if there are numerical instabilities.
1034 */
1035 public static INSTightlyCoupledKalmanState estimate(
1036 final Collection<GNSSMeasurement> measurements, final Time propagationInterval,
1037 final INSTightlyCoupledKalmanState previousState, final BodyKinematics bodyKinematics,
1038 final Angle previousLatitude, final INSTightlyCoupledKalmanConfig config) throws AlgebraException {
1039 return estimate(measurements, propagationInterval, previousState, bodyKinematics,
1040 convertAngle(previousLatitude), config);
1041 }
1042
1043 /**
1044 * Converts time instance into a value expressed in seconds.
1045 *
1046 * @param time time instance to be converted.
1047 * @return time value expressed in seconds.
1048 */
1049 private static double convertTime(final Time time) {
1050 return TimeConverter.convert(time.getValue().doubleValue(), time.getUnit(), TimeUnit.SECOND);
1051 }
1052
1053 /**
1054 * Converts angle instance into a value expressed in radians.
1055 *
1056 * @param angle angle instance to be converted.
1057 * @return angle value expressed in radians.
1058 */
1059 private static double convertAngle(final Angle angle) {
1060 return AngleConverter.convert(angle.getValue().doubleValue(), angle.getUnit(), AngleUnit.RADIANS);
1061 }
1062 }