1 /*
2 * Copyright (C) 2020 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.calibration.gyroscope;
17
18 import com.irurueta.algebra.AlgebraException;
19 import com.irurueta.algebra.Matrix;
20 import com.irurueta.algebra.Utils;
21 import com.irurueta.algebra.WrongSizeException;
22 import com.irurueta.navigation.LockedException;
23 import com.irurueta.navigation.NotReadyException;
24 import com.irurueta.navigation.inertial.BodyKinematics;
25 import com.irurueta.navigation.inertial.calibration.AngularSpeedTriad;
26 import com.irurueta.navigation.inertial.calibration.CalibrationException;
27 import com.irurueta.navigation.inertial.calibration.FrameBodyKinematics;
28 import com.irurueta.navigation.inertial.calibration.GyroscopeCalibrationSource;
29 import com.irurueta.navigation.inertial.estimators.ECEFKinematicsEstimator;
30 import com.irurueta.units.AngularSpeed;
31 import com.irurueta.units.AngularSpeedUnit;
32
33 import java.util.Collection;
34
35 /**
36 * Estimates gyroscope biases, cross couplings and scaling factors
37 * along with G-dependent cross biases introduced on the gyroscope by the
38 * specific forces sensed by the accelerometer.
39 * <p>
40 * This calibrator uses a linear approach to find a minimum least squared error
41 * solution.
42 * <p>
43 * To use this calibrator at least 7 measurements at different known frames must
44 * be provided. In other words, accelerometer and gyroscope (i.e. body kinematics)
45 * samples must be obtained at 7 different positions, orientations and velocities
46 * (although typically velocities are always zero).
47 * <p>
48 * Measured gyroscope angular rates is assumed to follow the model shown below:
49 * <pre>
50 * Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue + w
51 * </pre>
52 * Where:
53 * - Ωmeas is the measured gyroscope angular rates. This is a 3x1 vector.
54 * - bg is the gyroscope bias. Ideally, on a perfect gyroscope, this should be a
55 * 3x1 zero vector.
56 * - I is the 3x3 identity matrix.
57 * - Mg is the 3x3 matrix containing cross-couplings and scaling factors. Ideally, on
58 * a perfect gyroscope, this should be a 3x3 zero matrix.
59 * - Ωtrue is ground-truth gyroscope angular rates.
60 * - Gg is the G-dependent cross biases introduced by the specific forces sensed
61 * by the accelerometer. Ideally, on a perfect gyroscope, this should be a 3x3
62 * zero matrix.
63 * - ftrue is ground-truth specific force. This is a 3x1 vector.
64 * - w is measurement noise. This is a 3x1 vector.
65 */
66 @SuppressWarnings("DuplicatedCode")
67 public class KnownFrameGyroscopeLinearLeastSquaresCalibrator implements
68 KnownFrameGyroscopeCalibrator<FrameBodyKinematics, KnownFrameGyroscopeLinearLeastSquaresCalibratorListener>,
69 GyroscopeCalibrationSource, UnorderedFrameBodyKinematicsGyroscopeCalibrator {
70
71 /**
72 * Indicates whether by default a common z-axis is assumed for both the accelerometer
73 * and gyroscope.
74 */
75 public static final boolean DEFAULT_USE_COMMON_Z_AXIS = false;
76
77 /**
78 * Required minimum number of measurements.
79 */
80 public static final int MINIMUM_MEASUREMENTS = 7;
81
82 /**
83 * Number of equations generated for each measurement.
84 */
85 private static final int EQUATIONS_PER_MEASUREMENT = 3;
86
87 /**
88 * Number of unknowns when common z-axis is assumed for both the accelerometer
89 * and gyroscope.
90 */
91 private static final int COMMON_Z_AXIS_UNKNOWNS = 18;
92
93 /**
94 * Number of unknowns for the general case.
95 */
96 private static final int GENERAL_UNKNOWNS = 21;
97
98 /**
99 * Contains a collection of body kinematics measurements taken at different
100 * frames (positions, orientations and velocities).
101 * If a single device IMU needs to be calibrated, typically all measurements are
102 * taken at the same position, with zero velocity and multiple orientations.
103 * However, if we just want to calibrate a given IMU model (e.g. obtain
104 * an average and less precise calibration for the IMU of a given phone model),
105 * we could take measurements collected throughout the planet at multiple positions
106 * while the phone remains static (e.g. while charging), hence each measurement
107 * position will change, velocity will remain zero and orientation will be
108 * typically constant at horizontal orientation while the phone remains on a
109 * flat surface.
110 */
111 private Collection<FrameBodyKinematics> measurements;
112
113 /**
114 * This flag indicates whether z-axis is assumed to be common for accelerometer
115 * and gyroscope.
116 * When enabled, this eliminates 3 variables from Mg matrix.
117 */
118 private boolean commonAxisUsed = DEFAULT_USE_COMMON_Z_AXIS;
119
120 /**
121 * Listener to handle events raised by this calibrator.
122 */
123 private KnownFrameGyroscopeLinearLeastSquaresCalibratorListener listener;
124
125 /**
126 * Estimated angular rate biases for each IMU axis expressed in radians per
127 * second (rad/s).
128 */
129 private double[] estimatedBiases;
130
131 /**
132 * Estimated gyroscope scale factors and cross coupling errors.
133 * This is the product of matrix Tg containing cross coupling errors and Kg
134 * containing scaling factors.
135 * So that:
136 * <pre>
137 * Mg = [sx mxy mxz] = Tg*Kg
138 * [myx sy myz]
139 * [mzx mzy sz ]
140 * </pre>
141 * Where:
142 * <pre>
143 * Kg = [sx 0 0 ]
144 * [0 sy 0 ]
145 * [0 0 sz]
146 * </pre>
147 * and
148 * <pre>
149 * Tg = [1 -alphaXy alphaXz ]
150 * [alphaYx 1 -alphaYz]
151 * [-alphaZx alphaZy 1 ]
152 * </pre>
153 * Hence:
154 * <pre>
155 * Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * alphaXy sz * alphaXz ]
156 * [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
157 * [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
158 * </pre>
159 * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
160 * are considered to be zero if the gyroscope z-axis is assumed to be the same
161 * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
162 * becomes upper diagonal:
163 * <pre>
164 * Mg = [sx mxy mxz]
165 * [0 sy myz]
166 * [0 0 sz ]
167 * </pre>
168 * Values of this matrix are unit-less.
169 */
170 private Matrix estimatedMg;
171
172 /**
173 * Estimated G-dependent cross biases introduced on the gyroscope by the
174 * specific forces sensed by the accelerometer.
175 * This instance allows any 3x3 matrix.
176 */
177 private Matrix estimatedGg;
178
179 /**
180 * Indicates whether calibrator is running.
181 */
182 private boolean running;
183
184 /**
185 * Constructor.
186 */
187 public KnownFrameGyroscopeLinearLeastSquaresCalibrator() {
188 }
189
190 /**
191 * Constructor.
192 *
193 * @param listener listener to handle events raised by this calibrator.
194 */
195 public KnownFrameGyroscopeLinearLeastSquaresCalibrator(
196 final KnownFrameGyroscopeLinearLeastSquaresCalibratorListener listener) {
197 this.listener = listener;
198 }
199
200 /**
201 * Constructor.
202 *
203 * @param measurements collection of body kinematics measurements taken at
204 * different frames (positions, orientations and velocities).
205 */
206 public KnownFrameGyroscopeLinearLeastSquaresCalibrator(
207 final Collection<? extends FrameBodyKinematics> measurements) {
208 //noinspection unchecked
209 this.measurements = (Collection<FrameBodyKinematics>) measurements;
210 }
211
212 /**
213 * Constructor.
214 *
215 * @param measurements collection of body kinematics measurements taken at
216 * different frames (positions, orientations and velocities).
217 * @param listener listener to handle events raised by this calibrator.
218 */
219 public KnownFrameGyroscopeLinearLeastSquaresCalibrator(
220 final Collection<? extends FrameBodyKinematics> measurements,
221 final KnownFrameGyroscopeLinearLeastSquaresCalibratorListener listener) {
222 this(measurements);
223 this.listener = listener;
224 }
225
226 /**
227 * Constructor.
228 *
229 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
230 * accelerometer and gyroscope.
231 */
232 public KnownFrameGyroscopeLinearLeastSquaresCalibrator(final boolean commonAxisUsed) {
233 this.commonAxisUsed = commonAxisUsed;
234 }
235
236 /**
237 * Constructor.
238 *
239 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
240 * accelerometer and gyroscope.
241 * @param listener listener to handle events raised by this calibrator.
242 */
243 public KnownFrameGyroscopeLinearLeastSquaresCalibrator(
244 final boolean commonAxisUsed, final KnownFrameGyroscopeLinearLeastSquaresCalibratorListener listener) {
245 this(commonAxisUsed);
246 this.listener = listener;
247 }
248
249 /**
250 * Constructor.
251 *
252 * @param measurements collection of body kinematics measurements taken at
253 * different frames (positions, orientations and velocities).
254 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
255 * accelerometer and gyroscope.
256 */
257 public KnownFrameGyroscopeLinearLeastSquaresCalibrator(
258 final Collection<? extends FrameBodyKinematics> measurements, final boolean commonAxisUsed) {
259 this(measurements);
260 this.commonAxisUsed = commonAxisUsed;
261 }
262
263 /**
264 * Constructor.
265 *
266 * @param measurements collection of body kinematics measurements taken at
267 * different frames (positions, orientations and velocities).
268 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
269 * accelerometer and gyroscope.
270 * @param listener listener to handle events raised by this calibrator.
271 */
272 public KnownFrameGyroscopeLinearLeastSquaresCalibrator(
273 final Collection<? extends FrameBodyKinematics> measurements, final boolean commonAxisUsed,
274 final KnownFrameGyroscopeLinearLeastSquaresCalibratorListener listener) {
275 this(measurements, commonAxisUsed);
276 this.listener = listener;
277 }
278
279 /**
280 * Gets a collection of body kinematics measurements taken at different
281 * frames (positions, orientations and velocities).
282 * If a single device IMU needs to be calibrated, typically all measurements are
283 * taken at the same position, with zero velocity and multiple orientations.
284 * However, if we just want to calibrate a given IMU model (e.g. obtain
285 * an average and less precise calibration for the IMU of a given phone model),
286 * we could take measurements collected throughout the planet at multiple positions
287 * while the phone remains static (e.g. while charging), hence each measurement
288 * position will change, velocity will remain zero and orientation will be
289 * typically constant at horizontal orientation while the phone remains on a
290 * flat surface.
291 *
292 * @return a collection of body kinematics measurements taken at different
293 * frames (positions, orientations and velocities).
294 */
295 @Override
296 public Collection<FrameBodyKinematics> getMeasurements() {
297 return measurements;
298 }
299
300 /**
301 * Sets a collection of body kinematics measurements taken at different
302 * frames (positions, orientations and velocities).
303 * If a single device IMU needs to be calibrated, typically all measurements are
304 * taken at the same position, with zero velocity and multiple orientations.
305 * However, if we just want to calibrate the a given IMU model (e.g. obtain
306 * an average and less precise calibration for the IMU of a given phone model),
307 * we could take measurements collected throughout the planet at multiple positions
308 * while the phone remains static (e.g. while charging), hence each measurement
309 * position will change, velocity will remain zero and orientation will be
310 * typically constant at horizontal orientation while the phone remains on a
311 * flat surface.
312 *
313 * @param measurements collection of body kinematics measurements taken at different
314 * frames (positions, orientations and velocities).
315 * @throws LockedException if calibrator is currently running.
316 */
317 @Override
318 public void setMeasurements(final Collection<? extends FrameBodyKinematics> measurements) throws LockedException {
319 if (running) {
320 throw new LockedException();
321 }
322 //noinspection unchecked
323 this.measurements = (Collection<FrameBodyKinematics>) measurements;
324 }
325
326 /**
327 * Indicates the type of measurement or sequence used by this calibrator.
328 *
329 * @return type of measurement or sequence used by this calibrator.
330 */
331 @Override
332 public GyroscopeCalibratorMeasurementOrSequenceType getMeasurementOrSequenceType() {
333 return GyroscopeCalibratorMeasurementOrSequenceType.FRAME_BODY_KINEMATICS_MEASUREMENT;
334 }
335
336 /**
337 * Indicates whether this calibrator requires ordered measurements or sequences
338 * in a list or not.
339 *
340 * @return true if measurements or sequences must be ordered, false otherwise.
341 */
342 @Override
343 public boolean isOrderedMeasurementsOrSequencesRequired() {
344 return false;
345 }
346
347 /**
348 * Indicates whether this calibrator requires quality scores for each
349 * measurement/sequence or not.
350 *
351 * @return true if quality scores are required, false otherwise.
352 */
353 @Override
354 public boolean isQualityScoresRequired() {
355 return false;
356 }
357
358 /**
359 * Indicates whether z-axis is assumed to be common for accelerometer and
360 * gyroscope.
361 * When enabled, this eliminates 3 variables from Mg matrix.
362 *
363 * @return true if z-axis is assumed to be common for accelerometer and gyroscope,
364 * false otherwise.
365 */
366 @Override
367 public boolean isCommonAxisUsed() {
368 return commonAxisUsed;
369 }
370
371 /**
372 * Specifies whether z-axis is assumed to be common for accelerometer and
373 * gyroscope.
374 * When enabled, this eliminates 3 variables from Mg matrix.
375 *
376 * @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
377 * and gyroscope, false otherwise.
378 * @throws LockedException if calibrator is currently running.
379 */
380 @Override
381 public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
382 if (running) {
383 throw new LockedException();
384 }
385
386 this.commonAxisUsed = commonAxisUsed;
387 }
388
389 /**
390 * Gets listener to handle events raised by this calibrator.
391 *
392 * @return listener to handle events raised by this calibrator.
393 */
394 @Override
395 public KnownFrameGyroscopeLinearLeastSquaresCalibratorListener getListener() {
396 return listener;
397 }
398
399 /**
400 * Sets listener to handle events raised by this calibrator.
401 *
402 * @param listener listener to handle events raised by this calibrator.
403 * @throws LockedException if calibrator is currently running.
404 */
405 @Override
406 public void setListener(final KnownFrameGyroscopeLinearLeastSquaresCalibratorListener listener)
407 throws LockedException {
408 if (running) {
409 throw new LockedException();
410 }
411
412 this.listener = listener;
413 }
414
415 /**
416 * Gets minimum number of required measurements.
417 *
418 * @return minimum number of required measurements.
419 */
420 @Override
421 public int getMinimumRequiredMeasurementsOrSequences() {
422 return MINIMUM_MEASUREMENTS;
423 }
424
425 /**
426 * Indicates whether calibrator is ready to start the calibration.
427 *
428 * @return true if calibrator is ready, false otherwise.
429 */
430 @Override
431 public boolean isReady() {
432 return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
433 }
434
435 /**
436 * Indicates whether calibrator is currently running or not.
437 *
438 * @return true if calibrator is running, false otherwise.
439 */
440 @Override
441 public boolean isRunning() {
442 return running;
443 }
444
445 /**
446 * Estimates gyroscope calibration parameters containing bias, scale factors,
447 * cross-coupling errors and g-dependant cross biases.
448 *
449 * @throws LockedException if calibrator is currently running.
450 * @throws NotReadyException if calibrator is not ready.
451 * @throws CalibrationException if calibration fails for numerical reasons.
452 */
453 @Override
454 public void calibrate() throws LockedException, NotReadyException, CalibrationException {
455 if (running) {
456 throw new LockedException();
457 }
458
459 if (!isReady()) {
460 throw new NotReadyException();
461 }
462
463 try {
464 running = true;
465
466 if (listener != null) {
467 listener.onCalibrateStart(this);
468 }
469
470 if (commonAxisUsed) {
471 calibrateCommonAxis();
472 } else {
473 calibrateGeneral();
474 }
475
476 if (listener != null) {
477 listener.onCalibrateEnd(this);
478 }
479
480 } catch (final AlgebraException e) {
481 throw new CalibrationException(e);
482 } finally {
483 running = false;
484 }
485 }
486
487 /**
488 * Gets array containing x,y,z components of estimated gyroscope biases
489 * expressed in radians per second (rad/s).
490 *
491 * @return array containing x,y,z components of estimated gyroscope biases.
492 */
493 @Override
494 public double[] getEstimatedBiases() {
495 return estimatedBiases;
496 }
497
498 /**
499 * Gets array containing x,y,z components of estimated gyroscope biases
500 * expressed in radians per second (rad/s).
501 *
502 * @param result instance where estimated gyroscope biases will be stored.
503 * @return true if result instance was updated, false otherwise (when estimation
504 * is not yet available).
505 */
506 @Override
507 public boolean getEstimatedBiases(final double[] result) {
508 if (estimatedBiases != null) {
509 System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
510 return true;
511 } else {
512 return false;
513 }
514 }
515
516 /**
517 * Gets column matrix containing x,y,z components of estimated gyroscope biases
518 * expressed in radians per second (rad/s).
519 *
520 * @return column matrix containing x,y,z component of estimated gyroscope
521 * biases.
522 */
523 @Override
524 public Matrix getEstimatedBiasesAsMatrix() {
525 return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
526 }
527
528 /**
529 * Gets column matrix containing x,y,z components of estimated gyroscope biases
530 * expressed in radians per second (rad/s).
531 *
532 * @param result instance where result data will be stored.
533 * @return true if result was updated, false otherwise.
534 * @throws WrongSizeException if provided result instance has invalid size.
535 */
536 @Override
537 public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
538 if (estimatedBiases != null) {
539 result.fromArray(estimatedBiases);
540 return true;
541 } else {
542 return false;
543 }
544 }
545
546 /**
547 * Gets x coordinate of estimated gyroscope bias expressed in radians per second
548 * (rad/s).
549 *
550 * @return x coordinate of estimated gyroscope bias or null if not available.
551 */
552 @Override
553 public Double getEstimatedBiasX() {
554 return estimatedBiases != null ? estimatedBiases[0] : null;
555 }
556
557 /**
558 * Gets y coordinate of estimated gyroscope bias expressed in radians per second
559 * (rad/s).
560 *
561 * @return y coordinate of estimated gyroscope bias or null if not available.
562 */
563 @Override
564 public Double getEstimatedBiasY() {
565 return estimatedBiases != null ? estimatedBiases[1] : null;
566 }
567
568 /**
569 * Gets z coordinate of estimated gyroscope bias expressed in radians per second
570 * (rad/s).
571 *
572 * @return z coordinate of estimated gyroscope bias or null if not available.
573 */
574 @Override
575 public Double getEstimatedBiasZ() {
576 return estimatedBiases != null ? estimatedBiases[2] : null;
577 }
578
579 /**
580 * Gets x coordinate of estimated gyroscope bias.
581 *
582 * @return x coordinate of estimated gyroscope bias or null if not available.
583 */
584 @Override
585 public AngularSpeed getEstimatedBiasAngularSpeedX() {
586 return estimatedBiases != null
587 ? new AngularSpeed(estimatedBiases[0], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
588 }
589
590 /**
591 * Gets x coordinate of estimated gyroscope bias.
592 *
593 * @param result instance where result will be stored.
594 * @return true if result was updated, false if estimation is not available.
595 */
596 @Override
597 public boolean getEstimatedBiasAngularSpeedX(final AngularSpeed result) {
598 if (estimatedBiases != null) {
599 result.setValue(estimatedBiases[0]);
600 result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
601 return true;
602 } else {
603 return false;
604 }
605 }
606
607 /**
608 * Gets y coordinate of estimated gyroscope bias.
609 *
610 * @return y coordinate of estimated gyroscope bias or null if not available.
611 */
612 @Override
613 public AngularSpeed getEstimatedBiasAngularSpeedY() {
614 return estimatedBiases != null
615 ? new AngularSpeed(estimatedBiases[1], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
616 }
617
618 /**
619 * Gets y coordinate of estimated gyroscope bias.
620 *
621 * @param result instance where result will be stored.
622 * @return true if result was updated, false if estimation is not available.
623 */
624 @Override
625 public boolean getEstimatedBiasAngularSpeedY(final AngularSpeed result) {
626 if (estimatedBiases != null) {
627 result.setValue(estimatedBiases[1]);
628 result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
629 return true;
630 } else {
631 return false;
632 }
633 }
634
635 /**
636 * Gets z coordinate of estimated gyroscope bias.
637 *
638 * @return z coordinate of estimated gyroscope bias or null if not available.
639 */
640 @Override
641 public AngularSpeed getEstimatedBiasAngularSpeedZ() {
642 return estimatedBiases != null
643 ? new AngularSpeed(estimatedBiases[2], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
644 }
645
646 /**
647 * Gets z coordinate of estimated gyroscope bias.
648 *
649 * @param result instance where result will be stored.
650 * @return true if result was updated, false if estimation is not available.
651 */
652 @Override
653 public boolean getEstimatedBiasAngularSpeedZ(final AngularSpeed result) {
654 if (estimatedBiases != null) {
655 result.setValue(estimatedBiases[2]);
656 result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
657 return true;
658 } else {
659 return false;
660 }
661 }
662
663 /**
664 * Gets estimated gyroscope bias.
665 *
666 * @return estimated gyroscope bias or null if not available.
667 */
668 @Override
669 public AngularSpeedTriad getEstimatedBiasAsTriad() {
670 return estimatedBiases != null
671 ? new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND,
672 estimatedBiases[0], estimatedBiases[1], estimatedBiases[2])
673 : null;
674 }
675
676 /**
677 * Gets estimated gyroscope bias.
678 *
679 * @param result instance where result will be stored.
680 * @return true if estimated gyroscope bias is available and result was
681 * modified, false otherwise.
682 */
683 @Override
684 public boolean getEstimatedBiasAsTriad(final AngularSpeedTriad result) {
685 if (estimatedBiases != null) {
686 result.setValueCoordinatesAndUnit(
687 estimatedBiases[0], estimatedBiases[1], estimatedBiases[2], AngularSpeedUnit.RADIANS_PER_SECOND);
688 return true;
689 } else {
690 return false;
691 }
692 }
693
694 /**
695 * Gets estimated gyroscope scale factors and cross coupling errors.
696 * This is the product of matrix Tg containing cross coupling errors and Kg
697 * containing scaling factors.
698 * So that:
699 * <pre>
700 * Mg = [sx mxy mxz] = Tg*Kg
701 * [myx sy myz]
702 * [mzx mzy sz ]
703 * </pre>
704 * Where:
705 * <pre>
706 * Kg = [sx 0 0 ]
707 * [0 sy 0 ]
708 * [0 0 sz]
709 * </pre>
710 * and
711 * <pre>
712 * Tg = [1 -alphaXy alphaXz ]
713 * [alphaYx 1 -alphaYz]
714 * [-alphaZx alphaZy 1 ]
715 * </pre>
716 * Hence:
717 * <pre>
718 * Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * alphaXy sz * alphaXz ]
719 * [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
720 * [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
721 * </pre>
722 * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
723 * are considered to be zero if the gyroscope z-axis is assumed to be the same
724 * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
725 * becomes upper diagonal:
726 * <pre>
727 * Mg = [sx mxy mxz]
728 * [0 sy myz]
729 * [0 0 sz ]
730 * </pre>
731 * Values of this matrix are unit-less.
732 *
733 * @return estimated gyroscope scale factors and cross coupling errors.
734 */
735 @Override
736 public Matrix getEstimatedMg() {
737 return estimatedMg;
738 }
739
740 /**
741 * Gets estimated x-axis scale factor.
742 *
743 * @return estimated x-axis scale factor or null if not available.
744 */
745 @Override
746 public Double getEstimatedSx() {
747 return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
748 }
749
750 /**
751 * Gets estimated y-axis scale factor.
752 *
753 * @return estimated y-axis scale factor or null if not available.
754 */
755 @Override
756 public Double getEstimatedSy() {
757 return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
758 }
759
760 /**
761 * Gets estimated z-axis scale factor.
762 *
763 * @return estimated z-axis scale factor or null if not available.
764 */
765 @Override
766 public Double getEstimatedSz() {
767 return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
768 }
769
770 /**
771 * Gets estimated x-y cross-coupling error.
772 *
773 * @return estimated x-y cross-coupling error or null if not available.
774 */
775 @Override
776 public Double getEstimatedMxy() {
777 return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
778 }
779
780 /**
781 * Gets estimated x-z cross-coupling error.
782 *
783 * @return estimated x-z cross-coupling error or null if not available.
784 */
785 @Override
786 public Double getEstimatedMxz() {
787 return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
788 }
789
790 /**
791 * Gets estimated y-x cross-coupling error.
792 *
793 * @return estimated y-x cross-coupling error or null if not available.
794 */
795 @Override
796 public Double getEstimatedMyx() {
797 return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
798 }
799
800 /**
801 * Gets estimated y-z cross-coupling error.
802 *
803 * @return estimated y-z cross-coupling error or null if not available.
804 */
805 @Override
806 public Double getEstimatedMyz() {
807 return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
808 }
809
810 /**
811 * Gets estimated z-x cross-coupling error.
812 *
813 * @return estimated z-x cross-coupling error or null if not available.
814 */
815 @Override
816 public Double getEstimatedMzx() {
817 return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
818 }
819
820 /**
821 * Gets estimated z-y cross-coupling error.
822 *
823 * @return estimated z-y cross-coupling error or null if not available.
824 */
825 @Override
826 public Double getEstimatedMzy() {
827 return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
828 }
829
830 /**
831 * Gets estimated G-dependent cross biases introduced on the gyroscope by the
832 * specific forces sensed by the accelerometer.
833 *
834 * @return a 3x3 matrix containing g-dependent cross biases.
835 */
836 @Override
837 public Matrix getEstimatedGg() {
838 return estimatedGg;
839 }
840
841 /**
842 * Internal method to perform calibration when common z-axis is assumed for both
843 * the accelerometer and gyroscope.
844 *
845 * @throws AlgebraException if there are numerical errors.
846 */
847 private void calibrateCommonAxis() throws AlgebraException {
848 // The gyroscope model is:
849 // Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue + w
850
851 // Ideally a least squares solution tries to minimize noise component, so:
852 // Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue
853
854 // Hence:
855 // [Ωmeasx] = [bx] + ( [1 0 0] + [sx mxy mxz]) [Ωtruex] + [g11 g12 g13][ftruex]
856 // [Ωmeasy] [by] [0 1 0] [myx sy myz] [Ωtruey] [g21 g22 g23][ftruey]
857 // [Ωmeasz] [bz] [0 0 1] [mzx mzy sz ] [Ωtruez] [g31 g32 g33][ftruez]
858
859 // where myx = mzx = mzy = 0
860
861 // Hence:
862 // [Ωmeasx] = [bx] + ( [1 0 0] + [sx mxy mxz]) [Ωtruex] + [g11 g12 g13][ftruex]
863 // [Ωmeasy] [by] [0 1 0] [0 sy myz] [Ωtruey] [g21 g22 g23][ftruey]
864 // [Ωmeasz] [bz] [0 0 1] [0 0 sz ] [Ωtruez] [g31 g32 g33][ftruez]
865
866
867 // [Ωmeasx] = [bx] + ( [1+sx mxy mxz ]) [Ωtruex] + [g11 g12 g13][ftruex]
868 // [Ωmeasy] [by] [0 1+sy myz ] [Ωtruey] [g21 g22 g23][ftruey]
869 // [Ωmeasz] [bz] [0 0 1+sz] [Ωtruez] [g31 g32 g33][ftruez]
870
871 // Ωmeasx = bx + (1+sx) * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
872 // Ωmeasy = by + (1+sy) * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
873 // Ωmeasz = bz + (1+sz) * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez
874
875 // Where the unknowns are: bx, by, bz, sx, sy, sz, mxy mxz, myz, g11, g12, g13, g21, g22, g23, g31, g32, g33
876 // Reordering:
877 // Ωmeasx = bx + Ωtruex + sx * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
878 // Ωmeasy = by + Ωtruey + sy * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
879 // Ωmeasz = bz + Ωtruez + sz * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez
880
881 // Ωmeasx - Ωtruex = bx + sx * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
882 // Ωmeasy - Ωtruey = by + sy * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
883 // Ωmeasz - Ωtruez = bz + sz * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez
884
885 // [1 0 0 Ωtruex 0 0 Ωtruey Ωtruez 0 ftruex ftruey ftruez 0 0 0 0 0 0 ][bx ] = [Ωmeasx - Ωtruex]
886 // [0 1 0 0 Ωtruey 0 0 0 Ωtruez 0 0 0 ftruex ftruey ftruez 0 0 0 ][by ] [Ωmeasy - Ωtruey]
887 // [0 0 1 0 0 Ωtruez 0 0 0 0 0 0 0 0 0 ftruex ftruey ftruez][bz ] [Ωmeasz - Ωtruez]
888 // [sx ]
889 // [sy ]
890 // [sz ]
891 // [mxy]
892 // [mxz]
893 // [myz]
894 // [g11]
895 // [g12]
896 // [g13]
897 // [g21]
898 // [g22]
899 // [g23]
900 // [g31]
901 // [g32]
902 // [g33]
903
904 final var expectedKinematics = new BodyKinematics();
905
906 final var rows = EQUATIONS_PER_MEASUREMENT * measurements.size();
907 final var a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
908 final var b = new Matrix(rows, 1);
909 var i = 0;
910 for (final var measurement : measurements) {
911 final var measuredKinematics = measurement.getKinematics();
912 final var ecefFrame = measurement.getFrame();
913 final var previousEcefFrame = measurement.getPreviousFrame();
914 final var timeInterval = measurement.getTimeInterval();
915
916 ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame, previousEcefFrame, expectedKinematics);
917
918 final var omegaMeasX = measuredKinematics.getAngularRateX();
919 final var omegaMeasY = measuredKinematics.getAngularRateY();
920 final var omegaMeasZ = measuredKinematics.getAngularRateZ();
921
922 final var omegaTrueX = expectedKinematics.getAngularRateX();
923 final var omegaTrueY = expectedKinematics.getAngularRateY();
924 final var omegaTrueZ = expectedKinematics.getAngularRateZ();
925
926 final var fTrueX = expectedKinematics.getFx();
927 final var fTrueY = expectedKinematics.getFy();
928 final var fTrueZ = expectedKinematics.getFz();
929
930 a.setElementAt(i, 0, 1.0);
931 a.setElementAt(i, 3, omegaTrueX);
932 a.setElementAt(i, 6, omegaTrueY);
933 a.setElementAt(i, 7, omegaTrueZ);
934 a.setElementAt(i, 9, fTrueX);
935 a.setElementAt(i, 10, fTrueY);
936 a.setElementAt(i, 11, fTrueZ);
937
938 b.setElementAtIndex(i, omegaMeasX - omegaTrueX);
939 i++;
940
941 a.setElementAt(i, 1, 1.0);
942 a.setElementAt(i, 4, omegaTrueY);
943 a.setElementAt(i, 8, omegaTrueZ);
944 a.setElementAt(i, 12, fTrueX);
945 a.setElementAt(i, 13, fTrueY);
946 a.setElementAt(i, 14, fTrueZ);
947
948 b.setElementAtIndex(i, omegaMeasY - omegaTrueY);
949 i++;
950
951 a.setElementAt(i, 2, 1.0);
952 a.setElementAt(i, 5, omegaTrueZ);
953 a.setElementAt(i, 15, fTrueX);
954 a.setElementAt(i, 16, fTrueY);
955 a.setElementAt(i, 17, fTrueZ);
956
957 b.setElementAtIndex(i, omegaMeasZ - omegaTrueZ);
958 i++;
959 }
960
961 final var unknowns = Utils.solve(a, b);
962
963 final var bx = unknowns.getElementAtIndex(0);
964 final var by = unknowns.getElementAtIndex(1);
965 final var bz = unknowns.getElementAtIndex(2);
966 final var sx = unknowns.getElementAtIndex(3);
967 final var sy = unknowns.getElementAtIndex(4);
968 final var sz = unknowns.getElementAtIndex(5);
969 final var mxy = unknowns.getElementAtIndex(6);
970 final var mxz = unknowns.getElementAtIndex(7);
971 final var myz = unknowns.getElementAtIndex(8);
972 final var g11 = unknowns.getElementAtIndex(9);
973 final var g12 = unknowns.getElementAtIndex(10);
974 final var g13 = unknowns.getElementAtIndex(11);
975 final var g21 = unknowns.getElementAtIndex(12);
976 final var g22 = unknowns.getElementAtIndex(13);
977 final var g23 = unknowns.getElementAtIndex(14);
978 final var g31 = unknowns.getElementAtIndex(15);
979 final var g32 = unknowns.getElementAtIndex(16);
980 final var g33 = unknowns.getElementAtIndex(17);
981
982 fillBiases(bx, by, bz);
983 fillMg(sx, sy, sz, mxy, mxz, 0.0, myz, 0.0, 0.0);
984 fillGg(g11, g12, g13, g21, g22, g23, g31, g32, g33);
985 }
986
987 /**
988 * Internal method to perform general calibration.
989 *
990 * @throws AlgebraException if there are numerical errors.
991 */
992 private void calibrateGeneral() throws AlgebraException {
993 // The gyroscope model is:
994 // Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue + w
995
996 // Ideally a least squares solution tries to minimize noise component, so:
997 // Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue
998
999 // Hence:
1000 // [Ωmeasx] = [bx] + ( [1 0 0] + [sx mxy mxz]) [Ωtruex] + [g11 g12 g13][ftruex]
1001 // [Ωmeasy] [by] [0 1 0] [myx sy myz] [Ωtruey] [g21 g22 g23][ftruey]
1002 // [Ωmeasz] [bz] [0 0 1] [mzx mzy sz ] [Ωtruez] [g31 g32 g33][ftruez]
1003
1004 // [Ωmeasx] = [bx] + ( [1+sx mxy mxz ]) [Ωtruex] + [g11 g12 g13][ftruex]
1005 // [Ωmeasy] [by] [myx 1+sy myz ] [Ωtruey] [g21 g22 g23][ftruey]
1006 // [Ωmeasz] [bz] [mzx mzy 1+sz] [Ωtruez] [g31 g32 g33][ftruez]
1007
1008 // Ωmeasx = bx + (1+sx) * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
1009 // Ωmeasy = by + myx * Ωtruex + (1+sy) * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
1010 // Ωmeasz = bz + mzx * Ωtruex + mzy * Ωtruey + (1+sz) * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez
1011
1012 // Where the unknowns are: bx, by, bz, sx, sy, sz, mxy mxz, myx, myz, mzx, mzy, g11, g12, g13, g21, g22, g23,
1013 // g31, g32, g33
1014 // Reordering:
1015 // Ωmeasx = bx + Ωtruex + sx * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
1016 // Ωmeasy = by + myx * Ωtruex + Ωtruey + sy * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
1017 // Ωmeasz = bz + mzx * Ωtruex + mzy * Ωtruey + Ωtruez + sz * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez
1018
1019 // Ωmeasx - Ωtruex = bx + sx * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
1020 // Ωmeasy - Ωtruey = by + myx * Ωtruex + sy * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
1021 // Ωmeasz - Ωtruez = bz + mzx * Ωtruex + mzy * Ωtruey + sz * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez
1022
1023 // [1 0 0 Ωtruex 0 0 Ωtruey Ωtruez 0 0 0 0 ftruex ftruey ftruez 0 0 0 0 0 0 ][bx ] = [Ωmeasx - Ωtruex]
1024 // [0 1 0 0 Ωtruey 0 0 0 Ωtruex Ωtruez 0 0 0 0 0 ftruex ftruey ftruez 0 0 0 ][by ] [Ωmeasy - Ωtruey]
1025 // [0 0 1 0 0 Ωtruez 0 0 0 0 Ωtruex Ωtruey 0 0 0 0 0 0 ftruex ftruey ftruez][bz ] [Ωmeasz - Ωtruez]
1026 // [sx ]
1027 // [sy ]
1028 // [sz ]
1029 // [mxy]
1030 // [mxz]
1031 // [myx]
1032 // [myz]
1033 // [mzx]
1034 // [mzy]
1035 // [g11]
1036 // [g12]
1037 // [g13]
1038 // [g21]
1039 // [g22]
1040 // [g23]
1041 // [g31]
1042 // [g32]
1043 // [g33]
1044
1045 final var expectedKinematics = new BodyKinematics();
1046
1047 final var rows = EQUATIONS_PER_MEASUREMENT * measurements.size();
1048 final var a = new Matrix(rows, GENERAL_UNKNOWNS);
1049 final var b = new Matrix(rows, 1);
1050 var i = 0;
1051 for (final var measurement : measurements) {
1052 final var measuredKinematics = measurement.getKinematics();
1053 final var ecefFrame = measurement.getFrame();
1054 final var previousEcefFrame = measurement.getPreviousFrame();
1055 final var timeInterval = measurement.getTimeInterval();
1056
1057 ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame, previousEcefFrame, expectedKinematics);
1058
1059 final var omegaMeasX = measuredKinematics.getAngularRateX();
1060 final var omegaMeasY = measuredKinematics.getAngularRateY();
1061 final var omegaMeasZ = measuredKinematics.getAngularRateZ();
1062
1063 final var omegaTrueX = expectedKinematics.getAngularRateX();
1064 final var omegaTrueY = expectedKinematics.getAngularRateY();
1065 final var omegaTrueZ = expectedKinematics.getAngularRateZ();
1066
1067 final var fTrueX = expectedKinematics.getFx();
1068 final var fTrueY = expectedKinematics.getFy();
1069 final var fTrueZ = expectedKinematics.getFz();
1070
1071 a.setElementAt(i, 0, 1.0);
1072 a.setElementAt(i, 3, omegaTrueX);
1073 a.setElementAt(i, 6, omegaTrueY);
1074 a.setElementAt(i, 7, omegaTrueZ);
1075 a.setElementAt(i, 12, fTrueX);
1076 a.setElementAt(i, 13, fTrueY);
1077 a.setElementAt(i, 14, fTrueZ);
1078
1079 b.setElementAtIndex(i, omegaMeasX - omegaTrueX);
1080 i++;
1081
1082 a.setElementAt(i, 1, 1.0);
1083 a.setElementAt(i, 4, omegaTrueY);
1084 a.setElementAt(i, 8, omegaTrueX);
1085 a.setElementAt(i, 9, omegaTrueZ);
1086 a.setElementAt(i, 15, fTrueX);
1087 a.setElementAt(i, 16, fTrueY);
1088 a.setElementAt(i, 17, fTrueZ);
1089
1090 b.setElementAtIndex(i, omegaMeasY - omegaTrueY);
1091 i++;
1092
1093 a.setElementAt(i, 2, 1.0);
1094 a.setElementAt(i, 5, omegaTrueZ);
1095 a.setElementAt(i, 10, omegaTrueX);
1096 a.setElementAt(i, 11, omegaTrueY);
1097 a.setElementAt(i, 18, fTrueX);
1098 a.setElementAt(i, 19, fTrueY);
1099 a.setElementAt(i, 20, fTrueZ);
1100
1101 b.setElementAtIndex(i, omegaMeasZ - omegaTrueZ);
1102 i++;
1103 }
1104
1105 final var unknowns = Utils.solve(a, b);
1106
1107 final var bx = unknowns.getElementAtIndex(0);
1108 final var by = unknowns.getElementAtIndex(1);
1109 final var bz = unknowns.getElementAtIndex(2);
1110 final var sx = unknowns.getElementAtIndex(3);
1111 final var sy = unknowns.getElementAtIndex(4);
1112 final var sz = unknowns.getElementAtIndex(5);
1113 final var mxy = unknowns.getElementAtIndex(6);
1114 final var mxz = unknowns.getElementAtIndex(7);
1115 final var myx = unknowns.getElementAtIndex(8);
1116 final var myz = unknowns.getElementAtIndex(9);
1117 final var mzx = unknowns.getElementAtIndex(10);
1118 final var mzy = unknowns.getElementAtIndex(11);
1119 final var g11 = unknowns.getElementAtIndex(12);
1120 final var g12 = unknowns.getElementAtIndex(13);
1121 final var g13 = unknowns.getElementAtIndex(14);
1122 final var g21 = unknowns.getElementAtIndex(15);
1123 final var g22 = unknowns.getElementAtIndex(16);
1124 final var g23 = unknowns.getElementAtIndex(17);
1125 final var g31 = unknowns.getElementAtIndex(18);
1126 final var g32 = unknowns.getElementAtIndex(19);
1127 final var g33 = unknowns.getElementAtIndex(20);
1128
1129 fillBiases(bx, by, bz);
1130 fillMg(sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
1131 fillGg(g11, g12, g13, g21, g22, g23, g31, g32, g33);
1132 }
1133
1134 /**
1135 * Fills estimated biases array with estimated values.
1136 *
1137 * @param bx x coordinate of bias.
1138 * @param by y coordinate of bias.
1139 * @param bz z coordinate of bias.
1140 */
1141 private void fillBiases(final double bx, final double by, final double bz) {
1142 if (estimatedBiases == null) {
1143 estimatedBiases = new double[BodyKinematics.COMPONENTS];
1144 }
1145
1146 estimatedBiases[0] = bx;
1147 estimatedBiases[1] = by;
1148 estimatedBiases[2] = bz;
1149 }
1150
1151 /**
1152 * Fills scale factor and cross coupling error matrix with estimated values.
1153 *
1154 * @param sx x scale factor
1155 * @param sy y scale factor
1156 * @param sz z scale factor
1157 * @param mxy x-y cross coupling
1158 * @param mxz x-z cross coupling
1159 * @param myx y-x cross coupling
1160 * @param myz y-z cross coupling
1161 * @param mzx z-x cross coupling
1162 * @param mzy z-y cross coupling
1163 * @throws WrongSizeException never happens.
1164 */
1165 private void fillMg(final double sx, final double sy, final double sz,
1166 final double mxy, final double mxz, final double myx,
1167 final double myz, final double mzx, final double mzy) throws WrongSizeException {
1168 if (estimatedMg == null) {
1169 estimatedMg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
1170 }
1171
1172 estimatedMg.setElementAt(0, 0, sx);
1173 estimatedMg.setElementAt(1, 0, myx);
1174 estimatedMg.setElementAt(2, 0, mzx);
1175
1176 estimatedMg.setElementAt(0, 1, mxy);
1177 estimatedMg.setElementAt(1, 1, sy);
1178 estimatedMg.setElementAt(2, 1, mzy);
1179
1180 estimatedMg.setElementAt(0, 2, mxz);
1181 estimatedMg.setElementAt(1, 2, myz);
1182 estimatedMg.setElementAt(2, 2, sz);
1183 }
1184
1185 /**
1186 * Fills G-dependant cross biases.
1187 *
1188 * @param g11 element 1,1 of G-dependant cross biases matrix.
1189 * @param g12 element 1,2 of G-dependant cross biases matrix.
1190 * @param g13 element 1,3 of G-dependant cross biases matrix.
1191 * @param g21 element 2,1 of G-dependant cross biases matrix.
1192 * @param g22 element 2,2 of G-dependant cross biases matrix.
1193 * @param g23 element 2,3 of G-dependant cross biases matrix.
1194 * @param g31 element 3,1 of G-dependant cross biases matrix.
1195 * @param g32 element 3,2 of G-dependant cross biases matrix.
1196 * @param g33 element 3,3 of G-dependant cross biases matrix.
1197 * @throws WrongSizeException never happens.
1198 */
1199 private void fillGg(final double g11, final double g12, final double g13,
1200 final double g21, final double g22, final double g23,
1201 final double g31, final double g32, final double g33) throws WrongSizeException {
1202 if (estimatedGg == null) {
1203 estimatedGg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
1204 }
1205
1206 estimatedGg.setElementAt(0, 0, g11);
1207 estimatedGg.setElementAt(0, 1, g12);
1208 estimatedGg.setElementAt(0, 2, g13);
1209
1210 estimatedGg.setElementAt(1, 0, g21);
1211 estimatedGg.setElementAt(1, 1, g22);
1212 estimatedGg.setElementAt(1, 2, g23);
1213
1214 estimatedGg.setElementAt(2, 0, g31);
1215 estimatedGg.setElementAt(2, 1, g32);
1216 estimatedGg.setElementAt(2, 2, g33);
1217 }
1218 }