GyroscopeIntervalDetectorThresholdFactorOptimizer.java
/*
* Copyright (C) 2021 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.intervals.thresholdfactor;
import com.irurueta.algebra.Matrix;
import com.irurueta.navigation.LockedException;
import com.irurueta.navigation.NotReadyException;
import com.irurueta.navigation.inertial.INSLooselyCoupledKalmanInitializerConfig;
import com.irurueta.navigation.inertial.INSTightlyCoupledKalmanInitializerConfig;
import com.irurueta.navigation.inertial.calibration.AccelerometerNoiseRootPsdSource;
import com.irurueta.navigation.inertial.calibration.BodyKinematicsSequence;
import com.irurueta.navigation.inertial.calibration.CalibrationException;
import com.irurueta.navigation.inertial.calibration.GyroscopeBiasUncertaintySource;
import com.irurueta.navigation.inertial.calibration.GyroscopeCalibrationSource;
import com.irurueta.navigation.inertial.calibration.GyroscopeNoiseRootPsdSource;
import com.irurueta.navigation.inertial.calibration.StandardDeviationTimedBodyKinematics;
import com.irurueta.navigation.inertial.calibration.TimedBodyKinematics;
import com.irurueta.navigation.inertial.calibration.accelerometer.KnownBiasAccelerometerCalibrator;
import com.irurueta.navigation.inertial.calibration.generators.GyroscopeMeasurementsGenerator;
import com.irurueta.navigation.inertial.calibration.generators.GyroscopeMeasurementsGeneratorListener;
import com.irurueta.navigation.inertial.calibration.gyroscope.GyroscopeCalibratorMeasurementOrSequenceType;
import com.irurueta.navigation.inertial.calibration.gyroscope.GyroscopeNonLinearCalibrator;
import com.irurueta.navigation.inertial.calibration.gyroscope.OrderedBodyKinematicsSequenceGyroscopeCalibrator;
import com.irurueta.navigation.inertial.calibration.gyroscope.QualityScoredGyroscopeCalibrator;
import com.irurueta.navigation.inertial.calibration.gyroscope.UnknownBiasGyroscopeCalibrator;
import com.irurueta.navigation.inertial.calibration.intervals.TriadStaticIntervalDetector;
import com.irurueta.units.Acceleration;
import com.irurueta.units.AccelerationUnit;
import com.irurueta.units.Time;
import java.util.ArrayList;
import java.util.List;
/**
* Optimizes the threshold factor for interval detection of gyroscope data based
* on results of calibration.
* Implementations of this class will attempt to find the best threshold factor
* between the provided range of values.
* Only gyroscope calibrators based on unknown orientation are supported (in other terms,
* calibrators must be {@link GyroscopeNonLinearCalibrator} and must support
* {@link GyroscopeCalibratorMeasurementOrSequenceType#BODY_KINEMATICS_SEQUENCE}).
*/
public abstract class GyroscopeIntervalDetectorThresholdFactorOptimizer extends
IntervalDetectorThresholdFactorOptimizer<TimedBodyKinematics,
GyroscopeIntervalDetectorThresholdFactorOptimizerDataSource>
implements GyroscopeNoiseRootPsdSource, GyroscopeBiasUncertaintySource,
GyroscopeCalibrationSource, AccelerometerNoiseRootPsdSource {
/**
* Default minimum threshold factor.
*/
public static final double DEFAULT_MIN_THRESHOLD_FACTOR = 2.0;
/**
* Default maximum threshold factor.
*/
public static final double DEFAULT_MAX_THRESHOLD_FACTOR = 10.0;
/**
* Minimum threshold factor.
*/
protected double minThresholdFactor = DEFAULT_MIN_THRESHOLD_FACTOR;
/**
* Maximum threshold factor.
*/
protected double maxThresholdFactor = DEFAULT_MAX_THRESHOLD_FACTOR;
/**
* Gyroscope calibrator.
*/
private GyroscopeNonLinearCalibrator calibrator;
/**
* A measurement generator for gyroscope calibrators.
*/
private GyroscopeMeasurementsGenerator generator;
/**
* Generated sequences to be used for gyroscope calibration.
*/
private List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences;
/**
* Mapper to convert {@link BodyKinematicsSequence} sequences of {@link StandardDeviationTimedBodyKinematics}
* into quality scores.
*/
private QualityScoreMapper<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> qualityScoreMapper =
new DefaultGyroscopeQualityScoreMapper();
/**
* Estimated norm of gyroscope noise root PSD (Power Spectral Density)
* expressed as (rad * s^-0.5).
*/
private double angularSpeedNoiseRootPsd;
/**
* Accelerometer base noise level that has been detected during
* initialization of the best solution that has been found expressed in
* meters per squared second (m/s^2).
* This is equal to the standard deviation of the accelerometer measurements
* during the initialization phase.
*/
private double baseNoiseLevel;
/**
* Threshold to determine static/dynamic period changes expressed in
* meters per squared second (m/s^2) for the best calibration solution that
* has been found.
*/
private double threshold;
/**
* Estimated covariance matrix for estimated parameters.
*/
private Matrix estimatedCovariance;
/**
* Estimated angular rate biases for each IMU axis expressed in radians per
* second (rad/s).
*/
private double[] estimatedBiases;
/**
* 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;
/**
* Constructor.
*/
protected GyroscopeIntervalDetectorThresholdFactorOptimizer() {
initialize();
}
/**
* Constructor.
*
* @param dataSource instance in charge of retrieving data for this optimizer.
*/
protected GyroscopeIntervalDetectorThresholdFactorOptimizer(
final GyroscopeIntervalDetectorThresholdFactorOptimizerDataSource dataSource) {
super(dataSource);
initialize();
}
/**
* Constructor.
*
* @param calibrator a gyroscope calibrator to be used to optimize its
* Mean Square Error (MSE).
* @throws IllegalArgumentException if gyroscope calibrator does not use
* {@link BodyKinematicsSequence} sequences of
* {@link StandardDeviationTimedBodyKinematics}.
*/
protected GyroscopeIntervalDetectorThresholdFactorOptimizer(final GyroscopeNonLinearCalibrator calibrator) {
try {
setCalibrator(calibrator);
} catch (final LockedException ignore) {
// never happens
}
initialize();
}
/**
* Constructor.
*
* @param dataSource instance in charge of retrieving data for this optimizer.
* @param calibrator a gyroscope calibrator to be used to optimize its
* Mean Square Error (MSE).
* @throws IllegalArgumentException if gyroscope calibrator does not use
* {@link BodyKinematicsSequence} sequences of
* {@link StandardDeviationTimedBodyKinematics}.
*/
protected GyroscopeIntervalDetectorThresholdFactorOptimizer(
final GyroscopeIntervalDetectorThresholdFactorOptimizerDataSource dataSource,
final GyroscopeNonLinearCalibrator calibrator) {
super(dataSource);
try {
setCalibrator(calibrator);
} catch (final LockedException ignore) {
// never happens
}
initialize();
}
/**
* Gets provided gyroscope calibrator to be used to optimize its Mean Square Error (MSE).
*
* @return gyroscope calibrator to be used to optimize its MSE.
*/
public GyroscopeNonLinearCalibrator getCalibrator() {
return calibrator;
}
/**
* Sets gyroscope calibrator to be used to optimize its Mean Square Error (MSE).
*
* @param calibrator gyroscope calibrator to be use dto optimize its MSE.
* @throws LockedException if optimizer is already running.
* @throws IllegalArgumentException if gyroscope calibrator does not use
* {@link BodyKinematicsSequence} sequences of
* {@link StandardDeviationTimedBodyKinematics}.
*/
public void setCalibrator(final GyroscopeNonLinearCalibrator calibrator) throws LockedException {
if (running) {
throw new LockedException();
}
if (calibrator != null && calibrator.getMeasurementOrSequenceType()
!= GyroscopeCalibratorMeasurementOrSequenceType.BODY_KINEMATICS_SEQUENCE) {
throw new IllegalArgumentException();
}
this.calibrator = calibrator;
}
/**
* Gets mapper to convert {@link BodyKinematicsSequence} sequences of {@link StandardDeviationTimedBodyKinematics}
* into quality scores.
*
* @return mapper to convert sequences into quality scores.
*/
public QualityScoreMapper<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> getQualityScoreMapper() {
return qualityScoreMapper;
}
/**
* Sets mapper to convert {@link BodyKinematicsSequence} sequences of {@link StandardDeviationTimedBodyKinematics}
* into quality scores.
*
* @param qualityScoreMapper mapper to convert sequences into quality scores.
* @throws LockedException if optimizer is already running.
*/
public void setQualityScoreMapper(
final QualityScoreMapper<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> qualityScoreMapper)
throws LockedException {
if (running) {
throw new LockedException();
}
this.qualityScoreMapper = qualityScoreMapper;
}
/**
* Gets the minimum threshold factor.
*
* @return minimum threshold factor.
*/
public double getMinThresholdFactor() {
return minThresholdFactor;
}
/**
* Gets the maximum threshold factor.
*
* @return maximum threshold factor.
*/
public double getMaxThresholdFactor() {
return maxThresholdFactor;
}
/**
* Sets a range of threshold factor values to get an optimized
* threshold factor value.
*
* @param minThresholdFactor minimum threshold.
* @param maxThresholdFactor maximum threshold.
* @throws LockedException if optimizer is already running.
* @throws IllegalArgumentException if either minimum or maximum values are
* negative, or if the minimum value is larger
* than the maximum one.
*/
public void setThresholdFactorRange(final double minThresholdFactor, final double maxThresholdFactor)
throws LockedException {
if (running) {
throw new LockedException();
}
if (minThresholdFactor < 0.0 || maxThresholdFactor < 0.0 || minThresholdFactor >= maxThresholdFactor) {
throw new IllegalArgumentException();
}
this.minThresholdFactor = minThresholdFactor;
this.maxThresholdFactor = maxThresholdFactor;
}
/**
* Indicates whether this optimizer is ready to start optimization.
*
* @return true if this optimizer is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && calibrator != null && qualityScoreMapper != null;
}
/**
* Gets the time interval between input measurements provided to the
* {@link #getDataSource()} expressed in seconds (s).
*
* @return time interval between input measurements.
*/
public double getTimeInterval() {
return generator.getTimeInterval();
}
/**
* Sets time interval between input measurements provided to the
* {@link #getDataSource()} expressed in seconds (s).
*
* @param timeInterval time interval between input measurements.
* @throws LockedException if optimizer is already running.
* @throws IllegalArgumentException if provided value is negative.
*/
public void setTimeInterval(final double timeInterval) throws LockedException {
if (running) {
throw new LockedException();
}
generator.setTimeInterval(timeInterval);
}
/**
* Gets the time interval between input measurements provided to the
* {@link #getDataSource()}.
*
* @return time interval between input measurements.
*/
public Time getTimeIntervalAsTime() {
return generator.getTimeIntervalAsTime();
}
/**
* Gets the time interval between input measurements provided to the
* {@link #getDataSource()}.
*
* @param result instance where the time interval will be stored.
*/
public void getTimeIntervalAsTime(final Time result) {
generator.getTimeIntervalAsTime(result);
}
/**
* Sets time interval between input measurements provided to the
* {@link #getDataSource()}.
*
* @param timeInterval time interval between input measurements.
* @throws LockedException if optimizer is already running.
* @throws IllegalArgumentException if provided value is negative.
*/
public void setTimeInterval(final Time timeInterval) throws LockedException {
if (running) {
throw new LockedException();
}
generator.setTimeInterval(timeInterval);
}
/**
* Gets minimum number of input measurements provided to the
* {@link #getDataSource()} required in a static interval to be taken
* into account.
* Smaller static intervals will be discarded.
*
* @return a minimum number of input measurements required in a static interval
* to be taken into account.
*/
public int getMinStaticSamples() {
return generator.getMinStaticSamples();
}
/**
* Sets minimum number of input measurements provided to the
* {@link #getDataSource()} required in a static interval to be taken
* into account.
* Smaller static intervals will be discarded.
*
* @param minStaticSamples a minimum number of input measurements required in
* a static interval to be taken into account.
* @throws LockedException if optimizer is already running.
* @throws IllegalArgumentException if provided value is less than 2.
*/
public void setMinStaticSamples(final int minStaticSamples) throws LockedException {
if (running) {
throw new LockedException();
}
generator.setMinStaticSamples(minStaticSamples);
}
/**
* Gets maximum number of input measurements provided to the
* {@link #getDataSource()} allowed in dynamic intervals.
*
* @return maximum number of input measurements allowed in dynamic intervals.
*/
public int getMaxDynamicSamples() {
return generator.getMaxDynamicSamples();
}
/**
* Sets maximum number of input measurements provided to the
* {@link #getDataSource()} allowed in dynamic intervals.
*
* @param maxDynamicSamples maximum number of input measurements allowed in
* dynamic intervals.
* @throws LockedException if optimizer is already running.
* @throws IllegalArgumentException if provided value is less than 2.
*/
public void setMaxDynamicSamples(final int maxDynamicSamples) throws LockedException {
if (running) {
throw new LockedException();
}
generator.setMaxDynamicSamples(maxDynamicSamples);
}
/**
* Gets length of number of input measurements provided to the
* {@link #getDataSource()} to keep within the window being processed
* to determine instantaneous accelerometer noise level.
*
* @return length of input measurements to keep within the window.
*/
public int getWindowSize() {
return generator.getWindowSize();
}
/**
* Sets length of number of input measurements provided to the
* {@link #getDataSource()} to keep within the window being processed
* to determine instantaneous noise level.
* Window size must always be larger than the allowed minimum value, which is 2
* and must have an odd value.
*
* @param windowSize length of number of samples to keep within the window.
* @throws LockedException if optimizer is already running.
* @throws IllegalArgumentException if provided value is not valid.
*/
public void setWindowSize(final int windowSize) throws LockedException {
if (running) {
throw new LockedException();
}
generator.setWindowSize(windowSize);
}
/**
* Gets number of input measurements provided to the
* {@link #getDataSource()} to be processed initially while keeping the sensor
* static to find the base noise level when the device is static.
*
* @return number of samples to be processed initially.
*/
public int getInitialStaticSamples() {
return generator.getInitialStaticSamples();
}
/**
* Sets number of input parameters provided to the {@link #getDataSource()}
* to be processed initially while keeping the sensor static to
* find the base noise level when the device is static.
*
* @param initialStaticSamples number of samples ot be processed initially.
* @throws LockedException if optimizer is already running.
* @throws IllegalArgumentException if provided value is less than
* {@link TriadStaticIntervalDetector#MINIMUM_INITIAL_STATIC_SAMPLES}.
*/
public void setInitialStaticSamples(final int initialStaticSamples) throws LockedException {
if (running) {
throw new LockedException();
}
generator.setInitialStaticSamples(initialStaticSamples);
}
/**
* Gets factor to determine that a sudden movement has occurred during
* initialization if instantaneous noise level exceeds accumulated noise
* level by this factor amount.
* This factor is unit-less.
*
* @return factor to determine that a sudden movement has occurred.
*/
public double getInstantaneousNoiseLevelFactor() {
return generator.getInstantaneousNoiseLevelFactor();
}
/**
* Sets factor to determine that a sudden movement has occurred during
* initialization if instantaneous noise level exceeds accumulated noise
* level by this factor amount.
* This factor is unit-less.
*
* @param instantaneousNoiseLevelFactor factor to determine that a sudden
* movement has occurred during
* initialization.
* @throws LockedException if optimizer is already running.
* @throws IllegalArgumentException if provided value is zero or negative.
*/
public void setInstantaneousNoiseLevelFactor(final double instantaneousNoiseLevelFactor) throws LockedException {
if (running) {
throw new LockedException();
}
generator.setInstantaneousNoiseLevelFactor(instantaneousNoiseLevelFactor);
}
/**
* Gets the overall absolute threshold to determine whether there has been
* excessive motion during the whole initialization phase.
* This threshold is expressed in meters per squared second (m/s^2).
*
* @return overall absolute threshold to determine whether there has been
* excessive motion.
*/
public double getBaseNoiseLevelAbsoluteThreshold() {
return generator.getBaseNoiseLevelAbsoluteThreshold();
}
/**
* Sets the overall absolute threshold to determine whether there has been
* excessive motion during the whole initialization phase.
* This threshold is expressed in meters per squared second (m/s^2).
*
* @param baseNoiseLevelAbsoluteThreshold overall absolute threshold to
* determine whether there has been
* excessive motion.
* @throws LockedException if optimizer is already running.
* @throws IllegalArgumentException if provided value is zero or negative.
*/
public void setBaseNoiseLevelAbsoluteThreshold(final double baseNoiseLevelAbsoluteThreshold)
throws LockedException {
if (running) {
throw new LockedException();
}
generator.setBaseNoiseLevelAbsoluteThreshold(baseNoiseLevelAbsoluteThreshold);
}
/**
* Gets the overall absolute threshold to determine whether there has been
* excessive motion during the whole initialization phase.
*
* @return overall absolute threshold to determine whether there has been
* excessive motion.
*/
public Acceleration getBaseNoiseLevelAbsoluteThresholdAsMeasurement() {
return generator.getBaseNoiseLevelAbsoluteThresholdAsMeasurement();
}
/**
* Gets the overall absolute threshold to determine whether there has been
* excessive motion during the whole initialization phase.
*
* @param result instance where the result will be stored.
*/
public void getBaseNoiseLevelAbsoluteThresholdAsMeasurement(final Acceleration result) {
generator.getBaseNoiseLevelAbsoluteThresholdAsMeasurement(result);
}
/**
* Sets the overall absolute threshold to determine whether there has been
* excessive motion during the whole initialization phase.
*
* @param baseNoiseLevelAbsoluteThreshold overall absolute threshold to
* determine whether there has been
* excessive motion.
* @throws LockedException if optimizer is already running.
* @throws IllegalArgumentException if provided value is zero or negative.
*/
public void setBaseNoiseLevelAbsoluteThreshold(final Acceleration baseNoiseLevelAbsoluteThreshold)
throws LockedException {
if (running) {
throw new LockedException();
}
generator.setBaseNoiseLevelAbsoluteThreshold(baseNoiseLevelAbsoluteThreshold);
}
/**
* Gets accelerometer base noise level that has been detected during
* initialization of the best solution that has been found expressed in
* meters per squared second (m/s^2).
* This is equal to the standard deviation of the accelerometer measurements
* during the initialization phase.
*
* @return accelerometer base noise level of the best solution that has been
* found.
*/
public double getAccelerometerBaseNoiseLevel() {
return baseNoiseLevel;
}
/**
* Gets accelerometer base noise level that has been detected during
* initialization of the best solution that has been found.
* This is equal to the standard deviation of the accelerometer measurements
* during the initialization phase.
*
* @return accelerometer base noise level of the best solution that has been
* found.
*/
public Acceleration getAccelerometerBaseNoiseLevelAsMeasurement() {
return createMeasurement(baseNoiseLevel, getDefaultUnit());
}
/**
* Gets accelerometer base noise level that has been detected during
* initialization of the best solution that has been found.
* This is equal to the standard deviation of the accelerometer measurements
* during the initialization phase.
*
* @param result instance where the result will be stored.
*/
public void getAccelerometerBaseNoiseLevelAsMeasurement(final Acceleration result) {
result.setValue(baseNoiseLevel);
result.setUnit(getDefaultUnit());
}
/**
* Gets gyroscope base noise level PSD (Power Spectral Density)
* expressed in (rad^2/s).
*
* @return gyroscope base noise level PSD.
*/
public double getGyroscopeBaseNoiseLevelPsd() {
return angularSpeedNoiseRootPsd * angularSpeedNoiseRootPsd;
}
/**
* Gets gyroscope base noise level root PSD (Power Spectral Density)
* expressed in (rad * s^-0.5)
*
* @return gyroscope base noise level root PSD.
*/
@Override
public double getGyroscopeBaseNoiseLevelRootPsd() {
return angularSpeedNoiseRootPsd;
}
/**
* Gets accelerometer base noise level PSD (Power Spectral Density)
* expressed in (m^2 * s^-3).
*
* @return accelerometer base noise level PSD.
*/
public double getAccelerometerBaseNoiseLevelPsd() {
return baseNoiseLevel * baseNoiseLevel * getTimeInterval();
}
/**
* Gets accelerometer base noise level root PSD (Power Spectral Density)
* expressed in (m * s^-1.5).
*
* @return accelerometer base noise level root PSD.
*/
@Override
public double getAccelerometerBaseNoiseLevelRootPsd() {
return baseNoiseLevel * Math.sqrt(getTimeInterval());
}
/**
* Gets the threshold to determine static/dynamic period changes expressed in
* meters per squared second (m/s^2) for the best calibration solution that
* has been found.
*
* @return threshold to determine static/dynamic period changes for the best
* solution.
*/
public double getThreshold() {
return threshold;
}
/**
* Gets the threshold to determine static/dynamic period changes for the best
* calibration solution that has been found.
*
* @return threshold to determine static/dynamic period changes for the best
* solution.
*/
public Acceleration getThresholdAsMeasurement() {
return createMeasurement(threshold, getDefaultUnit());
}
/**
* Get the threshold to determine static/dynamic period changes for the best
* calibration solution that has been found.
*
* @param result instance where the result will be stored.
*/
public void getThresholdAsMeasurement(final Acceleration result) {
result.setValue(threshold);
result.setUnit(getDefaultUnit());
}
/**
* Gets estimated standard deviation norm of gyroscope bias expressed in
* radians per second (rad/s).
* This can be used as the initial gyroscope bias uncertainty for
* {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
*
* @return estimated standard deviation norm of gyroscope bias or null
* if not available.
*/
@Override
public Double getEstimatedBiasStandardDeviationNorm() {
return estimatedCovariance != null
? Math.sqrt(getEstimatedBiasXVariance() + getEstimatedBiasYVariance() + getEstimatedBiasZVariance())
: null;
}
/**
* Gets estimated x coordinate variance of gyroscope bias expressed in (rad^2/s^2).
*
* @return estimated x coordinate variance of gyroscope bias or null if not available.
*/
public Double getEstimatedBiasXVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(0, 0) : null;
}
/**
* Gets estimated y coordinate variance of gyroscope bias expressed in (rad^2/s^2).
*
* @return estimated y coordinate variance of gyroscope bias or null if not available.
*/
public Double getEstimatedBiasYVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(1, 1) : null;
}
/**
* Gets estimated z coordinate variance of gyroscope bias expressed in (rad^2/s^2).
*
* @return estimated z coordinate variance of gyroscope bias or null if not available.
*/
public Double getEstimatedBiasZVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(2, 2) : null;
}
/**
* Gets the array containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @return array containing x,y,z components of estimated gyroscope biases.
*/
@Override
public double[] getEstimatedBiases() {
return estimatedBiases;
}
/**
* 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 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;
}
/**
* Evaluates calibration Mean Square Error (MSE) for the provided threshold factor.
*
* @param thresholdFactor threshold factor to be used for interval detection
* and measurement generation to be used for
* calibration.
* @return calibration MSE.
* @throws LockedException if the generator is busy.
* @throws CalibrationException if calibration fails.
* @throws NotReadyException if the calibrator is not ready.
* @throws IntervalDetectorThresholdFactorOptimizerException interval detection failed.
*/
protected double evaluateForThresholdFactor(final double thresholdFactor) throws LockedException,
CalibrationException, NotReadyException, IntervalDetectorThresholdFactorOptimizerException {
if (sequences == null) {
sequences = new ArrayList<>();
} else {
sequences.clear();
}
generator.reset();
generator.setThresholdFactor(thresholdFactor);
var count = dataSource.count();
var failed = false;
for (var i = 0; i < count; i++) {
final var bodyKinematics = dataSource.getAt(i);
if (!generator.process(bodyKinematics)) {
failed = true;
break;
}
}
if (failed || generator.getStatus() == TriadStaticIntervalDetector.Status.FAILED) {
// interval detection failed
return Double.MAX_VALUE;
}
// check that enough measurements have been obtained
if (sequences.size() < calibrator.getMinimumRequiredMeasurementsOrSequences()) {
return Double.MAX_VALUE;
}
// set calibrator measurements
if (calibrator.getMeasurementOrSequenceType()
== GyroscopeCalibratorMeasurementOrSequenceType.BODY_KINEMATICS_SEQUENCE) {
final var cal = (OrderedBodyKinematicsSequenceGyroscopeCalibrator) this.calibrator;
cal.setSequences(sequences);
} else {
throw new IntervalDetectorThresholdFactorOptimizerException();
}
if (calibrator.isQualityScoresRequired()) {
final var cal = (QualityScoredGyroscopeCalibrator) this.calibrator;
final var size = sequences.size();
final var qualityScores = new double[size];
for (var i = 0; i < size; i++) {
qualityScores[i] = qualityScoreMapper.map(sequences.get(i));
}
cal.setQualityScores(qualityScores);
}
calibrator.calibrate();
final var mse = calibrator.getEstimatedMse();
if (mse < minMse) {
keepBestResult(mse, thresholdFactor);
}
return mse;
}
/**
* Initializes gyroscope measurement generator to convert timed body kinematics
* measurements after interval detection into measurements used for
* gyroscope calibration.
*/
private void initialize() {
final var generatorListener = new GyroscopeMeasurementsGeneratorListener() {
@Override
public void onInitializationStarted(final GyroscopeMeasurementsGenerator generator) {
// not needed
}
@Override
public void onInitializationCompleted(
final GyroscopeMeasurementsGenerator generator,
final double baseNoiseLevel) {
// not needed
}
@Override
public void onError(
final GyroscopeMeasurementsGenerator generator,
final TriadStaticIntervalDetector.ErrorReason reason) {
// not needed
}
@Override
public void onStaticIntervalDetected(final GyroscopeMeasurementsGenerator generator) {
// not needed
}
@Override
public void onDynamicIntervalDetected(final GyroscopeMeasurementsGenerator generator) {
// not needed
}
@Override
public void onStaticIntervalSkipped(final GyroscopeMeasurementsGenerator generator) {
// not needed
}
@Override
public void onDynamicIntervalSkipped(final GyroscopeMeasurementsGenerator generator) {
// not needed
}
@Override
public void onGeneratedMeasurement(
final GyroscopeMeasurementsGenerator generator,
final BodyKinematicsSequence<StandardDeviationTimedBodyKinematics> sequence) {
sequences.add(sequence);
}
@Override
public void onReset(final GyroscopeMeasurementsGenerator generator) {
// not needed
}
};
generator = new GyroscopeMeasurementsGenerator(generatorListener);
}
/**
* Keeps the best calibration solution found so far.
*
* @param mse Estimated Mean Square Error during calibration.
* @param thresholdFactor threshold factor to be kept.
*/
private void keepBestResult(final double mse, final double thresholdFactor) {
minMse = mse;
optimalThresholdFactor = thresholdFactor;
angularSpeedNoiseRootPsd = generator.getGyroscopeBaseNoiseLevelRootPsd();
baseNoiseLevel = generator.getAccelerometerBaseNoiseLevel();
threshold = generator.getThreshold();
if (estimatedCovariance == null) {
estimatedCovariance = new Matrix(calibrator.getEstimatedCovariance());
} else {
estimatedCovariance.copyFrom(calibrator.getEstimatedCovariance());
}
if (estimatedMg == null) {
estimatedMg = new Matrix(calibrator.getEstimatedMg());
} else {
estimatedMg.copyFrom(calibrator.getEstimatedMg());
}
if (estimatedGg == null) {
estimatedGg = new Matrix(calibrator.getEstimatedGg());
} else {
estimatedGg.copyFrom(calibrator.getEstimatedGg());
}
if (calibrator instanceof UnknownBiasGyroscopeCalibrator unknownBiasGyroscopeCalibrator) {
estimatedBiases = unknownBiasGyroscopeCalibrator.getEstimatedBiases();
} else if (calibrator instanceof KnownBiasAccelerometerCalibrator knownBiasAccelerometerCalibrator) {
estimatedBiases = knownBiasAccelerometerCalibrator.getBias();
}
}
/**
* Creates an acceleration instance using the provided value and unit.
*
* @param value value of measurement.
* @param unit unit of value.
* @return created acceleration.
*/
private Acceleration createMeasurement(final double value, final AccelerationUnit unit) {
return new Acceleration(value, unit);
}
/**
* Gets the default unit for acceleration, which is meters per
* squared second (m/s^2).
*
* @return default unit for acceleration.
*/
private AccelerationUnit getDefaultUnit() {
return AccelerationUnit.METERS_PER_SQUARED_SECOND;
}
}