KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java
/*
* Copyright (C) 2020 Alberto Irurueta Carro (alberto@irurueta.com)
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
package com.irurueta.navigation.inertial.calibration.gyroscope;
import com.irurueta.algebra.AlgebraException;
import com.irurueta.algebra.Matrix;
import com.irurueta.algebra.Utils;
import com.irurueta.algebra.WrongSizeException;
import com.irurueta.navigation.LockedException;
import com.irurueta.navigation.NotReadyException;
import com.irurueta.navigation.inertial.BodyKinematics;
import com.irurueta.navigation.inertial.calibration.AngularSpeedTriad;
import com.irurueta.navigation.inertial.calibration.CalibrationException;
import com.irurueta.navigation.inertial.calibration.FrameBodyKinematics;
import com.irurueta.navigation.inertial.estimators.ECEFKinematicsEstimator;
import com.irurueta.units.AngularSpeed;
import com.irurueta.units.AngularSpeedConverter;
import com.irurueta.units.AngularSpeedUnit;
import java.util.Collection;
/**
* Estimates gyroscope cross couplings and scaling factors
* along with G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
* This estimator assumes that biases are known.
* <p>
* This calibrator uses a linear approach to find a minimum least squared error
* solution.
* <p>
* To use this calibrator at least 6 measurements at different known frames must
* be provided. In other words, accelerometer and gyroscope (i.e. body kinematics)
* samples must be obtained at 6 different positions, orientations and velocities
* (although typically velocities are always zero).
* <p>
* Measured gyroscope angular rates is assumed to follow the model shown below:
* <pre>
* Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue + w
* </pre>
* Where:
* - Ωmeas is the measured gyroscope angular rates. This is a 3x1 vector.
* - bg is the gyroscope bias. This is a known 3x1 vector.
* - I is the 3x3 identity matrix.
* - Mg is the 3x3 matrix containing cross-couplings and scaling factors. Ideally, on
* a perfect gyroscope, this should be a 3x3 zero matrix.
* - Ωtrue is ground-truth gyroscope angular rates.
* - Gg is the G-dependent cross biases introduced by the specific forces sensed
* by the accelerometer. Ideally, on a perfect gyroscope, this should be a 3x3
* zero matrix.
* - ftrue is ground-truth specific force. This is a 3x1 vector.
* - w is measurement noise. This is a 3x1 vector.
*/
@SuppressWarnings("DuplicatedCode")
public class KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator implements
KnownBiasAndFrameGyroscopeCalibrator<FrameBodyKinematics,
KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibratorListener>,
UnorderedFrameBodyKinematicsGyroscopeCalibrator {
/**
* Indicates whether by default a common z-axis is assumed for both the accelerometer
* and gyroscope.
*/
public static final boolean DEFAULT_USE_COMMON_Z_AXIS = false;
/**
* Required minimum number of measurements.
*/
public static final int MINIMUM_MEASUREMENTS = 6;
/**
* Number of equations generated for each measurement.
*/
private static final int EQUATIONS_PER_MEASUREMENT = 3;
/**
* Number of unknowns when common z-axis is assumed for both the accelerometer
* and gyroscope.
*/
private static final int COMMON_Z_AXIS_UNKNOWNS = 15;
/**
* Number of unknowns for the general case.
*/
private static final int GENERAL_UNKNOWNS = 18;
/**
* Contains a collection of body kinematics measurements taken at different
* frames (positions, orientations and velocities).
* If a single device IMU needs to be calibrated, typically all measurements are
* taken at the same position, with zero velocity and multiple orientations.
* However, if we just want to calibrate a given IMU model (e.g. obtain
* an average and less precise calibration for the IMU of a given phone model),
* we could take measurements collected throughout the planet at multiple positions
* while the phone remains static (e.g. while charging), hence each measurement
* position will change, velocity will remain zero and orientation will be
* typically constant at horizontal orientation while the phone remains on a
* flat surface.
*/
private Collection<FrameBodyKinematics> measurements;
/**
* This flag indicates whether z-axis is assumed to be common for accelerometer
* and gyroscope.
* When enabled, this eliminates 3 variables from Mg matrix.
*/
private boolean commonAxisUsed = DEFAULT_USE_COMMON_Z_AXIS;
/**
* Listener to handle events raised by this calibrator.
*/
private KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibratorListener listener;
/**
* Known x coordinate of gyroscope bias expressed in radians per second (rad/s).
*/
private double biasX;
/**
* Known y coordinate of gyroscope bias expressed in radians per second (rad/s).
*/
private double biasY;
/**
* Known z coordinate of gyroscope bias expressed in radians per second (rad/s).
*/
private double biasZ;
/**
* Estimated gyroscope scale factors and cross coupling errors.
* This is the product of matrix Tg containing cross coupling errors and Kg
* containing scaling factors.
* So that:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Kg = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tg = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the gyroscope z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
* becomes upper diagonal:
* <pre>
* Mg = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*/
private Matrix estimatedMg;
/**
* Estimated G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
* This instance allows any 3x3 matrix.
*/
private Matrix estimatedGg;
/**
* Indicates whether calibrator is running.
*/
private boolean running;
/**
* Constructor.
*/
public KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator() {
}
/**
* Constructor.
*
* @param listener listener to handle events raised by this calibrator.
*/
public KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator(
final KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibratorListener listener) {
this.listener = listener;
}
/**
* Constructor.
*
* @param measurements collection of body kinematics measurements taken at
* different frames (positions, orientations and velocities).
*/
public KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator(final Collection<FrameBodyKinematics> measurements) {
this.measurements = measurements;
}
/**
* Constructor.
*
* @param measurements collection of body kinematics measurements taken at
* different frames (positions, orientations and velocities).
* @param listener listener to handle events raised by this calibrator.
*/
public KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator(
final Collection<FrameBodyKinematics> measurements,
final KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibratorListener listener) {
this(measurements);
this.listener = listener;
}
/**
* Constructor.
*
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
*/
public KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator(final boolean commonAxisUsed) {
this.commonAxisUsed = commonAxisUsed;
}
/**
* Constructor.
*
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param listener listener to handle events raised by this calibrator.
*/
public KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator(
final boolean commonAxisUsed,
final KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibratorListener listener) {
this(commonAxisUsed);
this.listener = listener;
}
/**
* Constructor.
*
* @param measurements collection of body kinematics measurements taken at
* different frames (positions, orientations and velocities).
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
*/
public KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator(
final Collection<FrameBodyKinematics> measurements, final boolean commonAxisUsed) {
this(measurements);
this.commonAxisUsed = commonAxisUsed;
}
/**
* Constructor.
*
* @param measurements collection of body kinematics measurements taken at
* different frames (positions, orientations and velocities).
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param listener listener to handle events raised by this calibrator.
*/
public KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator(
final Collection<FrameBodyKinematics> measurements, final boolean commonAxisUsed,
final KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibratorListener listener) {
this(measurements, commonAxisUsed);
this.listener = listener;
}
/**
* Constructor.
*
* @param biasX known x coordinate of gyroscope bias expressed in radians per second
* (rad/s).
* @param biasY known y coordinate of gyroscope bias expressed in radians per second
* (rad/s).
* @param biasZ known z coordinate of gyroscope bias expressed in radians per second
* (rad/s).
*/
public KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator(
final double biasX, final double biasY, final double biasZ) {
try {
setBiasCoordinates(biasX, biasY, biasZ);
} catch (final LockedException ignore) {
// never happens
}
}
/**
* Constructor.
*
* @param biasX known x coordinate of gyroscope bias expressed in radians per second
* (rad/s).
* @param biasY known y coordinate of gyroscope bias expressed in radians per second
* (rad/s).
* @param biasZ known z coordinate of gyroscope bias expressed in radians per second
* (rad/s).
* @param listener listener to handle events raised by this calibrator.
*/
public KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator(
final double biasX, final double biasY, final double biasZ,
final KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibratorListener listener) {
this(biasX, biasY, biasZ);
this.listener = listener;
}
/**
* Constructor.
*
* @param measurements collection of body kinematics measurements taken at
* different frames (positions, orientations and velocities).
* @param biasX known x coordinate of gyroscope bias expressed in radians per second
* (rad/s).
* @param biasY known y coordinate of gyroscope bias expressed in radians per second
* (rad/s).
* @param biasZ known z coordinate of gyroscope bias expressed in radians per second
* (rad/s).
*/
public KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator(
final Collection<FrameBodyKinematics> measurements,
final double biasX, final double biasY, final double biasZ) {
this(biasX, biasY, biasZ);
this.measurements = measurements;
}
/**
* Constructor.
*
* @param measurements collection of body kinematics measurements taken at
* different frames (positions, orientations and velocities).
* @param biasX known x coordinate of gyroscope bias expressed in radians per second
* (rad/s).
* @param biasY known y coordinate of gyroscope bias expressed in radians per second
* (rad/s).
* @param biasZ known z coordinate of gyroscope bias expressed in radians per second
* (rad/s).
* @param listener listener to handle events raised by this calibrator.
*/
public KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator(
final Collection<FrameBodyKinematics> measurements,
final double biasX, final double biasY, final double biasZ,
final KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibratorListener listener) {
this(measurements, biasX, biasY, biasZ);
this.listener = listener;
}
/**
* Constructor.
*
* @param biasX known x coordinate of gyroscope bias expressed in radians per second
* (rad/s).
* @param biasY known y coordinate of gyroscope bias expressed in radians per second
* (rad/s).
* @param biasZ known z coordinate of gyroscope bias expressed in radians per second
* (rad/s).
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
*/
public KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator(
final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed) {
this(biasX, biasY, biasZ);
this.commonAxisUsed = commonAxisUsed;
}
/**
* Constructor.
*
* @param biasX known x coordinate of gyroscope bias expressed in radians per second
* (rad/s).
* @param biasY known y coordinate of gyroscope bias expressed in radians per second
* (rad/s).
* @param biasZ known z coordinate of gyroscope bias expressed in radians per second
* (rad/s).
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param listener listener to handle events raised by this calibrator.
*/
public KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator(
final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed,
final KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibratorListener listener) {
this(biasX, biasY, biasZ, commonAxisUsed);
this.listener = listener;
}
/**
* Constructor.
*
* @param measurements collection of body kinematics measurements taken at
* different frames (positions, orientations and velocities).
* @param biasX known x coordinate of gyroscope bias expressed in radians per second
* (rad/s).
* @param biasY known y coordinate of gyroscope bias expressed in radians per second
* (rad/s).
* @param biasZ known z coordinate of gyroscope bias expressed in radians per second
* (rad/s).
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
*/
public KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator(
final Collection<FrameBodyKinematics> measurements,
final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed) {
this(measurements, biasX, biasY, biasZ);
this.commonAxisUsed = commonAxisUsed;
}
/**
* Constructor.
*
* @param measurements collection of body kinematics measurements taken at
* different frames (positions, orientations and velocities).
* @param biasX known x coordinate of gyroscope bias expressed in radians per second
* (rad/s).
* @param biasY known y coordinate of gyroscope bias expressed in radians per second
* (rad/s).
* @param biasZ known z coordinate of gyroscope bias expressed in radians per second
* (rad/s).
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param listener listener to handle events raised by this calibrator.
*/
public KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator(
final Collection<FrameBodyKinematics> measurements,
final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed,
final KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibratorListener listener) {
this(measurements, biasX, biasY, biasZ, commonAxisUsed);
this.listener = listener;
}
/**
* Constructor.
*
* @param biasX known x coordinate of gyroscope bias.
* @param biasY known y coordinate of gyroscope bias.
* @param biasZ known z coordinate of gyroscope bias.
*/
public KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator(
final AngularSpeed biasX, final AngularSpeed biasY, final AngularSpeed biasZ) {
try {
setBiasCoordinates(biasX, biasY, biasZ);
} catch (final LockedException ignore) {
// never happens
}
}
/**
* Constructor.
*
* @param biasX known x coordinate of gyroscope bias.
* @param biasY known y coordinate of gyroscope bias.
* @param biasZ known z coordinate of gyroscope bias.
* @param listener listener to handle events raised by this calibrator.
*/
public KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator(
final AngularSpeed biasX, final AngularSpeed biasY, final AngularSpeed biasZ,
final KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibratorListener listener) {
this(biasX, biasY, biasZ);
this.listener = listener;
}
/**
* Constructor.
*
* @param measurements collection of body kinematics measurements taken at
* different frames (positions, orientations and velocities).
* @param biasX known x coordinate of gyroscope bias.
* @param biasY known y coordinate of gyroscope bias.
* @param biasZ known z coordinate of gyroscope bias.
*/
public KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator(
final Collection<FrameBodyKinematics> measurements,
final AngularSpeed biasX, final AngularSpeed biasY, final AngularSpeed biasZ) {
this(biasX, biasY, biasZ);
this.measurements = measurements;
}
/**
* Constructor.
*
* @param measurements collection of body kinematics measurements taken at
* different frames (positions, orientations and velocities).
* @param biasX known x coordinate of gyroscope bias.
* @param biasY known y coordinate of gyroscope bias.
* @param biasZ known z coordinate of gyroscope bias.
* @param listener listener to handle events raised by this calibrator.
*/
public KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator(
final Collection<FrameBodyKinematics> measurements,
final AngularSpeed biasX, final AngularSpeed biasY, final AngularSpeed biasZ,
final KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibratorListener listener) {
this(measurements, biasX, biasY, biasZ);
this.listener = listener;
}
/**
* Constructor.
*
* @param biasX known x coordinate of gyroscope bias.
* @param biasY known y coordinate of gyroscope bias.
* @param biasZ known z coordinate of gyroscope bias.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
*/
public KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator(
final AngularSpeed biasX, final AngularSpeed biasY, final AngularSpeed biasZ,
final boolean commonAxisUsed) {
this(biasX, biasY, biasZ);
this.commonAxisUsed = commonAxisUsed;
}
/**
* Constructor.
*
* @param biasX known x coordinate of gyroscope bias.
* @param biasY known y coordinate of gyroscope bias.
* @param biasZ known z coordinate of gyroscope bias.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param listener listener to handle events raised by this calibrator.
*/
public KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator(
final AngularSpeed biasX, final AngularSpeed biasY, final AngularSpeed biasZ, final boolean commonAxisUsed,
final KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibratorListener listener) {
this(biasX, biasY, biasZ, commonAxisUsed);
this.listener = listener;
}
/**
* Constructor.
*
* @param measurements collection of body kinematics measurements taken at
* different frames (positions, orientations and velocities).
* @param biasX known x coordinate of gyroscope bias.
* @param biasY known y coordinate of gyroscope bias.
* @param biasZ known z coordinate of gyroscope bias.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
*/
public KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator(
final Collection<FrameBodyKinematics> measurements,
final AngularSpeed biasX, final AngularSpeed biasY, final AngularSpeed biasZ,
final boolean commonAxisUsed) {
this(measurements, biasX, biasY, biasZ);
this.commonAxisUsed = commonAxisUsed;
}
/**
* Constructor.
*
* @param measurements collections of body kinematics measurements taken at
* different frames (positions, orientations and velocities).
* @param biasX known x coordinate of gyroscope bias.
* @param biasY known y coordinate of gyroscope bias.
* @param biasZ known z coordinate of gyroscope bias.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param listener listener to handle events raised by this calibrator.
*/
public KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator(
final Collection<FrameBodyKinematics> measurements,
final AngularSpeed biasX, final AngularSpeed biasY, final AngularSpeed biasZ, final boolean commonAxisUsed,
final KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibratorListener listener) {
this(measurements, biasX, biasY, biasZ, commonAxisUsed);
this.listener = listener;
}
/**
* Gets a collection of body kinematics measurements taken at different
* frames (positions, orientations and velocities).
* If a single device IMU needs to be calibrated, typically all measurements are
* taken at the same position, with zero velocity and multiple orientations.
* However, if we just want to calibrate a given IMU model (e.g. obtain
* an average and less precise calibration for the IMU of a given phone model),
* we could take measurements collected throughout the planet at multiple positions
* while the phone remains static (e.g. while charging), hence each measurement
* position will change, velocity will remain zero and orientation will be
* typically constant at horizontal orientation while the phone remains on a
* flat surface.
*
* @return a collection of body kinematics measurements taken at different
* frames (positions, orientations and velocities).
*/
@Override
public Collection<FrameBodyKinematics> getMeasurements() {
return measurements;
}
/**
* Sets a collection of body kinematics measurements taken at different
* frames (positions, orientations and velocities).
* If a single device IMU needs to be calibrated, typically all measurements are
* taken at the same position, with zero velocity and multiple orientations.
* However, if we just want to calibrate the a given IMU model (e.g. obtain
* an average and less precise calibration for the IMU of a given phone model),
* we could take measurements collected throughout the planet at multiple positions
* while the phone remains static (e.g. while charging), hence each measurement
* position will change, velocity will remain zero and orientation will be
* typically constant at horizontal orientation while the phone remains on a
* flat surface.
*
* @param measurements collection of body kinematics measurements taken at different
* frames (positions, orientations and velocities).
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setMeasurements(final Collection<? extends FrameBodyKinematics> measurements) throws LockedException {
if (running) {
throw new LockedException();
}
//noinspection unchecked
this.measurements = (Collection<FrameBodyKinematics>) measurements;
}
/**
* Indicates the type of measurement or sequence used by this calibrator.
*
* @return type of measurement or sequence used by this calibrator.
*/
@Override
public GyroscopeCalibratorMeasurementOrSequenceType getMeasurementOrSequenceType() {
return GyroscopeCalibratorMeasurementOrSequenceType.FRAME_BODY_KINEMATICS_MEASUREMENT;
}
/**
* Indicates whether this calibrator requires ordered measurements or sequences
* in a list or not.
*
* @return true if measurements or sequences must be ordered, false otherwise.
*/
@Override
public boolean isOrderedMeasurementsOrSequencesRequired() {
return false;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement/sequence or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return false;
}
/**
* Indicates whether z-axis is assumed to be common for accelerometer and
* gyroscope.
* When enabled, this eliminates 3 variables from Mg matrix.
*
* @return true if z-axis is assumed to be common for accelerometer and gyroscope,
* false otherwise.
*/
@Override
public boolean isCommonAxisUsed() {
return commonAxisUsed;
}
/**
* Specifies whether z-axis is assumed to be common for accelerometer and
* gyroscope.
* When enabled, this eliminates 3 variables from Mg matrix.
*
* @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
* and gyroscope, false otherwise.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
if (running) {
throw new LockedException();
}
this.commonAxisUsed = commonAxisUsed;
}
/**
* Gets listener to handle events raised by this calibrator.
*
* @return listener to handle events raised by this calibrator.
*/
@Override
public KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibratorListener getListener() {
return listener;
}
/**
* Sets listener to handle events raised by this calibrator.
*
* @param listener listener to handle events raised by this calibrator.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setListener(final KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibratorListener listener)
throws LockedException {
if (running) {
throw new LockedException();
}
this.listener = listener;
}
/**
* Gets known x coordinate of gyroscope bias expressed in radians per second
* (rad/s).
*
* @return x coordinate of gyroscope bias.
*/
@Override
public double getBiasX() {
return biasX;
}
/**
* Sets known x coordinate of gyroscope bias expressed in radians per second
* (rad/s).
*
* @param biasX x coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasX(final double biasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = biasX;
}
/**
* Gets known y coordinate of gyroscope bias expressed in radians per second
* (rad/s).
*
* @return y coordinate of gyroscope bias.
*/
@Override
public double getBiasY() {
return biasY;
}
/**
* Sets known y coordinate of gyroscope bias expressed in radians per second
* (rad/s).
*
* @param biasY y coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasY(final double biasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasY = biasY;
}
/**
* Gets known z coordinate of gyroscope bias expressed in radians per second
* (rad/s).
*
* @return z coordinate of gyroscope bias.
*/
@Override
public double getBiasZ() {
return biasZ;
}
/**
* Sets known z coordinate of gyroscope bias expressed in radians per second
* (rad/s).
*
* @param biasZ z coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasZ(final double biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasZ = biasZ;
}
/**
* Gets known x coordinate of gyroscope bias.
*
* @return x coordinate of gyroscope bias.
*/
@Override
public AngularSpeed getBiasAngularSpeedX() {
return new AngularSpeed(biasX, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets known x coordinate of gyroscope bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasAngularSpeedX(final AngularSpeed result) {
result.setValue(biasX);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets known x coordinate of gyroscope bias.
*
* @param biasX x coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasX(final AngularSpeed biasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = convertAngularSpeed(biasX);
}
/**
* Gets known y coordinate of gyroscope bias.
*
* @return y coordinate of gyroscope bias.
*/
@Override
public AngularSpeed getBiasAngularSpeedY() {
return new AngularSpeed(biasY, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets known y coordinate of gyroscope bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasAngularSpeedY(final AngularSpeed result) {
result.setValue(biasY);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets known y coordinate of gyroscope bias.
*
* @param biasY y coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasY(final AngularSpeed biasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasY = convertAngularSpeed(biasY);
}
/**
* Gets known z coordinate of gyroscope bias.
*
* @return z coordinate of gyroscope bias.
*/
@Override
public AngularSpeed getBiasAngularSpeedZ() {
return new AngularSpeed(biasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets known z coordinate of gyroscope bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasAngularSpeedZ(final AngularSpeed result) {
result.setValue(biasZ);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets known z coordinate of gyroscope bias.
*
* @param biasZ z coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasZ(final AngularSpeed biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasZ = convertAngularSpeed(biasZ);
}
/**
* Sets known gyroscope bias coordinates expressed in radians per second
* (rad/s).
*
* @param biasX x coordinate of gyroscope bias.
* @param biasY y coordinate of gyroscope bias.
* @param biasZ z coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasCoordinates(final double biasX, final double biasY, final double biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = biasX;
this.biasY = biasY;
this.biasZ = biasZ;
}
/**
* Sets known gyroscope bias coordinates.
*
* @param biasX x coordinate of gyroscope bias.
* @param biasY y coordinate of gyroscope bias.
* @param biasZ z coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasCoordinates(final AngularSpeed biasX, final AngularSpeed biasY, final AngularSpeed biasZ)
throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = convertAngularSpeed(biasX);
this.biasY = convertAngularSpeed(biasY);
this.biasZ = convertAngularSpeed(biasZ);
}
/**
* Gets known gyroscope bias.
*
* @return known gyroscope bias.
*/
public AngularSpeedTriad getBiasAsTriad() {
return new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND, biasX, biasY, biasZ);
}
/**
* Gets known gyroscope bias.
*
* @param result instance where result will be stored.
*/
public void getBiasAsTriad(final AngularSpeedTriad result) {
result.setValueCoordinatesAndUnit(biasX, biasY, biasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets known gyroscope bias.
*
* @param bias gyroscope bias to be set.
* @throws LockedException if calibrator is currently running.
*/
public void setBias(final AngularSpeedTriad bias) throws LockedException {
if (running) {
throw new LockedException();
}
biasX = convertAngularSpeed(bias.getValueX(), bias.getUnit());
biasY = convertAngularSpeed(bias.getValueY(), bias.getUnit());
biasZ = convertAngularSpeed(bias.getValueZ(), bias.getUnit());
}
/**
* Gets known gyroscope bias as an array.
* Array values are expressed in radians per second (rad/s).
*
* @return array containing coordinate of known bias.
*/
@Override
public double[] getBias() {
final var result = new double[BodyKinematics.COMPONENTS];
getBias(result);
return result;
}
/**
* Gets known gyroscope bias as an array.
* Array values are expressed in radians per second (rad/s).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void getBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = biasX;
result[1] = biasY;
result[2] = biasZ;
}
/**
* Sets known gyroscope bias as an array.
* Array values are expressed in radians per second (rad/s).
*
* @param bias known gyroscope bias.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void setBias(final double[] bias) throws LockedException {
if (running) {
throw new LockedException();
}
if (bias.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
biasX = bias[0];
biasY = bias[1];
biasZ = bias[2];
}
/**
* Gets known gyroscope bias as a column matrix.
*
* @return known gyroscope bias as a column matrix.
*/
@Override
public Matrix getBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known gyroscope bias as a column matrix.
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, biasX);
result.setElementAtIndex(1, biasY);
result.setElementAtIndex(2, biasZ);
}
/**
* Sets known gyroscope bias as a column matrix.
*
* @param bias gyroscope bias to be set.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setBias(final Matrix bias) throws LockedException {
if (running) {
throw new LockedException();
}
if (bias.getRows() != BodyKinematics.COMPONENTS || bias.getColumns() != 1) {
throw new IllegalArgumentException();
}
biasX = bias.getElementAtIndex(0);
biasY = bias.getElementAtIndex(1);
biasZ = bias.getElementAtIndex(2);
}
/**
* Gets minimum number of required measurements.
*
* @return minimum number of required measurements.
*/
@Override
public int getMinimumRequiredMeasurementsOrSequences() {
return MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is ready to start the calibration.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Estimates gyroscope calibration parameters containing bias, scale factors,
* cross-coupling errors and g-dependant cross biases.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if calibration fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
if (commonAxisUsed) {
calibrateCommonAxis();
} else {
calibrateGeneral();
}
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final AlgebraException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Gets estimated gyroscope scale factors and cross coupling errors.
* This is the product of matrix Tg containing cross coupling errors and Kg
* containing scaling factors.
* So that:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Kg = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tg = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the gyroscope z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
* becomes upper diagonal:
* <pre>
* Mg = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated gyroscope scale factors and cross coupling errors.
*/
@Override
public Matrix getEstimatedMg() {
return estimatedMg;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
}
/**
* Gets estimated G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @return a 3x3 matrix containing g-dependent cross biases.
*/
@Override
public Matrix getEstimatedGg() {
return estimatedGg;
}
/**
* Internal method to perform calibration when common z-axis is assumed for both
* the accelerometer and gyroscope.
*
* @throws AlgebraException if there are numerical errors.
*/
private void calibrateCommonAxis() throws AlgebraException {
// The gyroscope model is:
// Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue + w
// Ideally a least squares solution tries to minimize noise component, so:
// Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue
// Hence:
// [Ωmeasx] = [bx] + ( [1 0 0] + [sx mxy mxz]) [Ωtruex] + [g11 g12 g13][ftruex]
// [Ωmeasy] [by] [0 1 0] [myx sy myz] [Ωtruey] [g21 g22 g23][ftruey]
// [Ωmeasz] [bz] [0 0 1] [mzx mzy sz ] [Ωtruez] [g31 g32 g33][ftruez]
// where myx = mzx = mzy = 0
// Hence:
// [Ωmeasx] = [bx] + ( [1 0 0] + [sx mxy mxz]) [Ωtruex] + [g11 g12 g13][ftruex]
// [Ωmeasy] [by] [0 1 0] [0 sy myz] [Ωtruey] [g21 g22 g23][ftruey]
// [Ωmeasz] [bz] [0 0 1] [0 0 sz ] [Ωtruez] [g31 g32 g33][ftruez]
// [Ωmeasx] = [bx] + ( [1+sx mxy mxz ]) [Ωtruex] + [g11 g12 g13][ftruex]
// [Ωmeasy] [by] [0 1+sy myz ] [Ωtruey] [g21 g22 g23][ftruey]
// [Ωmeasz] [bz] [0 0 1+sz] [Ωtruez] [g31 g32 g33][ftruez]
// Ωmeasx = bx + (1+sx) * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
// Ωmeasy = by + (1+sy) * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
// Ωmeasz = bz + (1+sz) * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez
// Where the unknowns are: sx, sy, sz, mxy mxz, myz, g11, g12, g13, g21, g22, g23, g31, g32, g33
// Reordering:
// Ωmeasx = bx + Ωtruex + sx * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
// Ωmeasy = by + Ωtruey + sy * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
// Ωmeasz = bz + Ωtruez + sz * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez
// Ωmeasx - Ωtruex - bx = sx * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
// Ωmeasy - Ωtruey - by = sy * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
// Ωmeasz - Ωtruez - bz = sz * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez
// [Ωtruex 0 0 Ωtruey Ωtruez 0 ftruex ftruey ftruez 0 0 0 0 0 0 ][sx ] = [Ωmeasx - Ωtruex - bx]
// [0 Ωtruey 0 0 0 Ωtruez 0 0 0 ftruex ftruey ftruez 0 0 0 ][sy ] [Ωmeasy - Ωtruey - by]
// [0 0 Ωtruez 0 0 0 0 0 0 0 0 0 ftruex ftruey ftruez][sz ] [Ωmeasz - Ωtruez - bz]
// [mxy]
// [mxz]
// [myz]
// [g11]
// [g12]
// [g13]
// [g21]
// [g22]
// [g23]
// [g31]
// [g32]
// [g33]
final var expectedKinematics = new BodyKinematics();
final var rows = EQUATIONS_PER_MEASUREMENT * measurements.size();
final var a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
final var b = new Matrix(rows, 1);
var i = 0;
for (final var measurement : measurements) {
final var measuredKinematics = measurement.getKinematics();
final var ecefFrame = measurement.getFrame();
final var previousEcefFrame = measurement.getPreviousFrame();
final var timeInterval = measurement.getTimeInterval();
ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame, previousEcefFrame, expectedKinematics);
final var omegaMeasX = measuredKinematics.getAngularRateX();
final var omegaMeasY = measuredKinematics.getAngularRateY();
final var omegaMeasZ = measuredKinematics.getAngularRateZ();
final var omegaTrueX = expectedKinematics.getAngularRateX();
final var omegaTrueY = expectedKinematics.getAngularRateY();
final var omegaTrueZ = expectedKinematics.getAngularRateZ();
final var fTrueX = expectedKinematics.getFx();
final var fTrueY = expectedKinematics.getFy();
final var fTrueZ = expectedKinematics.getFz();
a.setElementAt(i, 0, omegaTrueX);
a.setElementAt(i, 3, omegaTrueY);
a.setElementAt(i, 4, omegaTrueZ);
a.setElementAt(i, 6, fTrueX);
a.setElementAt(i, 7, fTrueY);
a.setElementAt(i, 8, fTrueZ);
b.setElementAtIndex(i, omegaMeasX - omegaTrueX - biasX);
i++;
a.setElementAt(i, 1, omegaTrueY);
a.setElementAt(i, 5, omegaTrueZ);
a.setElementAt(i, 9, fTrueX);
a.setElementAt(i, 10, fTrueY);
a.setElementAt(i, 11, fTrueZ);
b.setElementAtIndex(i, omegaMeasY - omegaTrueY - biasY);
i++;
a.setElementAt(i, 2, omegaTrueZ);
a.setElementAt(i, 12, fTrueX);
a.setElementAt(i, 13, fTrueY);
a.setElementAt(i, 14, fTrueZ);
b.setElementAtIndex(i, omegaMeasZ - omegaTrueZ - biasZ);
i++;
}
final var unknowns = Utils.solve(a, b);
final var sx = unknowns.getElementAtIndex(0);
final var sy = unknowns.getElementAtIndex(1);
final var sz = unknowns.getElementAtIndex(2);
final var mxy = unknowns.getElementAtIndex(3);
final var mxz = unknowns.getElementAtIndex(4);
final var myz = unknowns.getElementAtIndex(5);
final var g11 = unknowns.getElementAtIndex(6);
final var g12 = unknowns.getElementAtIndex(7);
final var g13 = unknowns.getElementAtIndex(8);
final var g21 = unknowns.getElementAtIndex(9);
final var g22 = unknowns.getElementAtIndex(10);
final var g23 = unknowns.getElementAtIndex(11);
final var g31 = unknowns.getElementAtIndex(12);
final var g32 = unknowns.getElementAtIndex(13);
final var g33 = unknowns.getElementAtIndex(14);
fillMg(sx, sy, sz, mxy, mxz, 0.0, myz, 0.0, 0.0);
fillGg(g11, g12, g13, g21, g22, g23, g31, g32, g33);
}
/**
* Internal method to perform general calibration.
*
* @throws AlgebraException if there are numerical errors.
*/
private void calibrateGeneral() throws AlgebraException {
// The gyroscope model is:
// Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue + w
// Ideally a least squares solution tries to minimize noise component, so:
// Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue
// Hence:
// [Ωmeasx] = [bx] + ( [1 0 0] + [sx mxy mxz]) [Ωtruex] + [g11 g12 g13][ftruex]
// [Ωmeasy] [by] [0 1 0] [myx sy myz] [Ωtruey] [g21 g22 g23][ftruey]
// [Ωmeasz] [bz] [0 0 1] [mzx mzy sz ] [Ωtruez] [g31 g32 g33][ftruez]
// [Ωmeasx] = [bx] + ( [1+sx mxy mxz ]) [Ωtruex] + [g11 g12 g13][ftruex]
// [Ωmeasy] [by] [myx 1+sy myz ] [Ωtruey] [g21 g22 g23][ftruey]
// [Ωmeasz] [bz] [mzx mzy 1+sz] [Ωtruez] [g31 g32 g33][ftruez]
// Ωmeasx = bx + (1+sx) * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
// Ωmeasy = by + myx * Ωtruex + (1+sy) * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
// Ωmeasz = bz + mzx * Ωtruex + mzy * Ωtruey + (1+sz) * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez
// Where the unknowns are: sx, sy, sz, mxy mxz, myx, myz, mzx, mzy, g11, g12, g13, g21, g22, g23,
// g31, g32, g33
// Reordering:
// Ωmeasx = bx + Ωtruex + sx * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
// Ωmeasy = by + myx * Ωtruex + Ωtruey + sy * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
// Ωmeasz = bz + mzx * Ωtruex + mzy * Ωtruey + Ωtruez + sz * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez
// Ωmeasx - Ωtruex - bx = sx * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
// Ωmeasy - Ωtruey - by = myx * Ωtruex + sy * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
// Ωmeasz - Ωtruez - bz = mzx * Ωtruex + mzy * Ωtruey + sz * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez
// [Ωtruex 0 0 Ωtruey Ωtruez 0 0 0 0 ftruex ftruey ftruez 0 0 0 0 0 0 ][sx ] = [Ωmeasx - Ωtruex - bx]
// [0 Ωtruey 0 0 0 Ωtruex Ωtruez 0 0 0 0 0 ftruex ftruey ftruez 0 0 0 ][sy ] [Ωmeasy - Ωtruey - by]
// [0 0 Ωtruez 0 0 0 0 Ωtruex Ωtruey 0 0 0 0 0 0 ftruex ftruey ftruez][sz ] [Ωmeasz - Ωtruez - bz]
// [mxy]
// [mxz]
// [myx]
// [myz]
// [mzx]
// [mzy]
// [g11]
// [g12]
// [g13]
// [g21]
// [g22]
// [g23]
// [g31]
// [g32]
// [g33]
final var expectedKinematics = new BodyKinematics();
final var rows = EQUATIONS_PER_MEASUREMENT * measurements.size();
final var a = new Matrix(rows, GENERAL_UNKNOWNS);
final var b = new Matrix(rows, 1);
var i = 0;
for (final var measurement : measurements) {
final var measuredKinematics = measurement.getKinematics();
final var ecefFrame = measurement.getFrame();
final var previousEcefFrame = measurement.getPreviousFrame();
final var timeInterval = measurement.getTimeInterval();
ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame, previousEcefFrame, expectedKinematics);
final var omegaMeasX = measuredKinematics.getAngularRateX();
final var omegaMeasY = measuredKinematics.getAngularRateY();
final var omegaMeasZ = measuredKinematics.getAngularRateZ();
final var omegaTrueX = expectedKinematics.getAngularRateX();
final var omegaTrueY = expectedKinematics.getAngularRateY();
final var omegaTrueZ = expectedKinematics.getAngularRateZ();
final var fTrueX = expectedKinematics.getFx();
final var fTrueY = expectedKinematics.getFy();
final var fTrueZ = expectedKinematics.getFz();
a.setElementAt(i, 0, omegaTrueX);
a.setElementAt(i, 3, omegaTrueY);
a.setElementAt(i, 4, omegaTrueZ);
a.setElementAt(i, 9, fTrueX);
a.setElementAt(i, 10, fTrueY);
a.setElementAt(i, 11, fTrueZ);
b.setElementAtIndex(i, omegaMeasX - omegaTrueX - biasX);
i++;
a.setElementAt(i, 1, omegaTrueY);
a.setElementAt(i, 5, omegaTrueX);
a.setElementAt(i, 6, omegaTrueZ);
a.setElementAt(i, 12, fTrueX);
a.setElementAt(i, 13, fTrueY);
a.setElementAt(i, 14, fTrueZ);
b.setElementAtIndex(i, omegaMeasY - omegaTrueY - biasY);
i++;
a.setElementAt(i, 2, omegaTrueZ);
a.setElementAt(i, 7, omegaTrueX);
a.setElementAt(i, 8, omegaTrueY);
a.setElementAt(i, 15, fTrueX);
a.setElementAt(i, 16, fTrueY);
a.setElementAt(i, 17, fTrueZ);
b.setElementAtIndex(i, omegaMeasZ - omegaTrueZ - biasZ);
i++;
}
final var unknowns = Utils.solve(a, b);
final var sx = unknowns.getElementAtIndex(0);
final var sy = unknowns.getElementAtIndex(1);
final var sz = unknowns.getElementAtIndex(2);
final var mxy = unknowns.getElementAtIndex(3);
final var mxz = unknowns.getElementAtIndex(4);
final var myx = unknowns.getElementAtIndex(5);
final var myz = unknowns.getElementAtIndex(6);
final var mzx = unknowns.getElementAtIndex(7);
final var mzy = unknowns.getElementAtIndex(8);
final var g11 = unknowns.getElementAtIndex(9);
final var g12 = unknowns.getElementAtIndex(10);
final var g13 = unknowns.getElementAtIndex(11);
final var g21 = unknowns.getElementAtIndex(12);
final var g22 = unknowns.getElementAtIndex(13);
final var g23 = unknowns.getElementAtIndex(14);
final var g31 = unknowns.getElementAtIndex(15);
final var g32 = unknowns.getElementAtIndex(16);
final var g33 = unknowns.getElementAtIndex(17);
fillMg(sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
fillGg(g11, g12, g13, g21, g22, g23, g31, g32, g33);
}
/**
* Fills scale factor and cross coupling error matrix with estimated values.
*
* @param sx x scale factor
* @param sy y scale factor
* @param sz z scale factor
* @param mxy x-y cross coupling
* @param mxz x-z cross coupling
* @param myx y-x cross coupling
* @param myz y-z cross coupling
* @param mzx z-x cross coupling
* @param mzy z-y cross coupling
* @throws WrongSizeException never happens.
*/
private void fillMg(final double sx, final double sy, final double sz,
final double mxy, final double mxz, final double myx,
final double myz, final double mzx, final double mzy) throws WrongSizeException {
if (estimatedMg == null) {
estimatedMg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
}
estimatedMg.setElementAt(0, 0, sx);
estimatedMg.setElementAt(1, 0, myx);
estimatedMg.setElementAt(2, 0, mzx);
estimatedMg.setElementAt(0, 1, mxy);
estimatedMg.setElementAt(1, 1, sy);
estimatedMg.setElementAt(2, 1, mzy);
estimatedMg.setElementAt(0, 2, mxz);
estimatedMg.setElementAt(1, 2, myz);
estimatedMg.setElementAt(2, 2, sz);
}
/**
* Fills G-dependant cross biases.
*
* @param g11 element 1,1 of G-dependant cross biases matrix.
* @param g12 element 1,2 of G-dependant cross biases matrix.
* @param g13 element 1,3 of G-dependant cross biases matrix.
* @param g21 element 2,1 of G-dependant cross biases matrix.
* @param g22 element 2,2 of G-dependant cross biases matrix.
* @param g23 element 2,3 of G-dependant cross biases matrix.
* @param g31 element 3,1 of G-dependant cross biases matrix.
* @param g32 element 3,2 of G-dependant cross biases matrix.
* @param g33 element 3,3 of G-dependant cross biases matrix.
* @throws WrongSizeException never happens.
*/
private void fillGg(final double g11, final double g12, final double g13,
final double g21, final double g22, final double g23,
final double g31, final double g32, final double g33) throws WrongSizeException {
if (estimatedGg == null) {
estimatedGg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
}
estimatedGg.setElementAt(0, 0, g11);
estimatedGg.setElementAt(0, 1, g12);
estimatedGg.setElementAt(0, 2, g13);
estimatedGg.setElementAt(1, 0, g21);
estimatedGg.setElementAt(1, 1, g22);
estimatedGg.setElementAt(1, 2, g23);
estimatedGg.setElementAt(2, 0, g31);
estimatedGg.setElementAt(2, 1, g32);
estimatedGg.setElementAt(2, 2, g33);
}
/**
* Converts angular speed instance to radians per second (rad/s).
*
* @param value angular speed value.
* @param unit unit of angular speed value.
* @return converted value.
*/
private static double convertAngularSpeed(final double value, final AngularSpeedUnit unit) {
return AngularSpeedConverter.convert(value, unit, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Converts angular speed instance to radians per second (rad/s).
*
* @param angularSpeed angular speed instance to be converted.
* @return converted value.
*/
private static double convertAngularSpeed(final AngularSpeed angularSpeed) {
return convertAngularSpeed(angularSpeed.getValue().doubleValue(), angularSpeed.getUnit());
}
}