DriftEstimator.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;
import com.irurueta.algebra.AlgebraException;
import com.irurueta.algebra.Matrix;
import com.irurueta.geometry.InvalidRotationMatrixException;
import com.irurueta.geometry.Quaternion;
import com.irurueta.geometry.Rotation3D;
import com.irurueta.navigation.LockedException;
import com.irurueta.navigation.NotReadyException;
import com.irurueta.navigation.frames.CoordinateTransformation;
import com.irurueta.navigation.frames.ECEFFrame;
import com.irurueta.navigation.frames.ECEFPosition;
import com.irurueta.navigation.frames.ECEFVelocity;
import com.irurueta.navigation.frames.InvalidSourceAndDestinationFrameTypeException;
import com.irurueta.navigation.frames.NEDFrame;
import com.irurueta.navigation.frames.NEDPosition;
import com.irurueta.navigation.frames.NEDVelocity;
import com.irurueta.navigation.frames.converters.ECEFtoNEDFrameConverter;
import com.irurueta.navigation.frames.converters.NEDtoECEFFrameConverter;
import com.irurueta.navigation.inertial.BodyKinematics;
import com.irurueta.navigation.inertial.navigators.ECEFInertialNavigator;
import com.irurueta.navigation.inertial.navigators.InertialNavigatorException;
import com.irurueta.units.Acceleration;
import com.irurueta.units.AccelerationUnit;
import com.irurueta.units.Angle;
import com.irurueta.units.AngleUnit;
import com.irurueta.units.AngularSpeed;
import com.irurueta.units.AngularSpeedUnit;
import com.irurueta.units.Distance;
import com.irurueta.units.DistanceUnit;
import com.irurueta.units.Speed;
import com.irurueta.units.SpeedUnit;
import com.irurueta.units.Time;
import com.irurueta.units.TimeConverter;
import com.irurueta.units.TimeUnit;
/**
* Estimates accumulated drift in body orientation, position and velocity per
* unit of time.
* This estimator must be executed while the body where the IMU is placed remains
* static.
*/
public class DriftEstimator {
/**
* Default time interval between kinematics samples expressed in seconds (s).
*/
public static final double DEFAULT_TIME_INTERVAL_SECONDS = 0.02;
/**
* Indicates whether this estimator is running.
*/
protected boolean running;
/**
* Number of processed body kinematics samples.
*/
protected int numberOfProcessedSamples;
/**
* Time interval expressed in seconds (s) between body kinematics samples.
*/
protected double timeInterval = DEFAULT_TIME_INTERVAL_SECONDS;
/**
* Fixes body kinematics measurements using accelerometer and gyroscope
* calibration data to fix measurements.
*/
protected final BodyKinematicsFixer fixer = new BodyKinematicsFixer();
/**
* Instance containing the last fixed body kinematics to be reused.
*/
protected final BodyKinematics fixedKinematics = new BodyKinematics();
/**
* Indicates whether measured kinematics must be fixed or not.
* When enabled, provided calibration data is used; otherwise it is
* ignored.
* By default, this is enabled.
*/
protected boolean fixKinematics = true;
/**
* Listener to handle events raised by this estimator.
*/
protected DriftEstimatorListener listener;
/**
* Initial frame containing body position, velocity and orientation expressed
* in ECEF coordinates before starting drift estimation.
*/
protected ECEFFrame referenceFrame;
/**
* Contains the current frame after one navigation step.
* This is reused for efficiency.
*/
protected final ECEFFrame frame = new ECEFFrame();
/**
* Contains orientation of a reference frame.
* This is reused for efficiency.
*/
protected final Quaternion refQ = new Quaternion();
/**
* Contains orientation inverse of the reference frame.
* This is reused for efficiency.
*/
protected final Quaternion invRefQ = new Quaternion();
/**
* Contains current frame orientation drift.
* This is reused for efficiency.
*/
protected final Quaternion q = new Quaternion();
/**
* Contains current position drift.
*/
protected final ECEFPosition currentPositionDrift = new ECEFPosition();
/**
* Contains current velocity drift.
*/
protected final ECEFVelocity currentVelocityDrift = new ECEFVelocity();
/**
* Contains current orientation expressed as a 3D rotation matrix.
*/
protected Matrix currentC;
/**
* Current position drift expressed in meters (m).
*/
protected double currentPositionDriftMeters;
/**
* Current velocity drift expressed in meters per second (m/s).
*/
protected double currentVelocityDriftMetersPerSecond;
/**
* Current orientation drift expressed in radians (rad).
*/
protected double currentOrientationDriftRadians;
/**
* Constructor.
*/
public DriftEstimator() {
}
/**
* Constructor.
*
* @param listener listener to handle events.
*/
public DriftEstimator(final DriftEstimatorListener listener) {
this.listener = listener;
}
/**
* Constructor.
*
* @param referenceFrame initial frame containing body position, velocity and
* orientation expressed in ECEF coordinates.
*/
public DriftEstimator(final ECEFFrame referenceFrame) {
this.referenceFrame = referenceFrame;
}
/**
* Constructor.
*
* @param referenceFrame initial frame containing body position, velocity and
* orientation expressed in ECEF coordinates.
* @param listener listener to handle events.
*/
public DriftEstimator(final ECEFFrame referenceFrame,
final DriftEstimatorListener listener) {
this.referenceFrame = referenceFrame;
this.listener = listener;
}
/**
* Constructor.
*
* @param referenceFrame initial frame containing body position, velocity and
* orientation expressed in NED coordinates.
*/
public DriftEstimator(final NEDFrame referenceFrame) {
try {
setReferenceNedFrame(referenceFrame);
} catch (final LockedException ignore) {
// never happens
}
}
/**
* Constructor.
*
* @param referenceFrame initial frame containing body position, velocity and
* orientation expressed in NED coordinates.
* @param listener listener to handle events.
*/
public DriftEstimator(final NEDFrame referenceFrame,
final DriftEstimatorListener listener) {
this(referenceFrame);
this.listener = listener;
}
/**
* Constructor.
*
* @param ba acceleration bias to be set.
* @param ma acceleration cross-coupling errors matrix. Must be 3x3.
* @param bg angular speed bias to be set.
* @param mg angular speed cross-coupling errors matrix. Must be 3x3.
* @throws AlgebraException if provided cross-coupling matrices cannot
* be inverted.
* @throws IllegalArgumentException if any provided matrices are not 3x3.
*/
public DriftEstimator(final AccelerationTriad ba,
final Matrix ma,
final AngularSpeedTriad bg,
final Matrix mg) throws AlgebraException {
try {
setAccelerationBias(ba);
setAccelerationCrossCouplingErrors(ma);
setAngularSpeedBias(bg);
setAngularSpeedCrossCouplingErrors(mg);
} catch (final LockedException ignore) {
// never happens
}
}
/**
* Constructor.
*
* @param ba acceleration bias to be set.
* @param ma acceleration cross-coupling errors matrix. Must be 3x3.
* @param bg angular speed bias to be set.
* @param mg angular speed cross-coupling errors matrix. Must be 3x3.
* @param listener listener to handle events.
* @throws AlgebraException if provided cross-coupling matrices cannot
* be inverted.
* @throws IllegalArgumentException if any provided matrices are not 3x3.
*/
public DriftEstimator(
final AccelerationTriad ba,
final Matrix ma,
final AngularSpeedTriad bg,
final Matrix mg,
final DriftEstimatorListener listener) throws AlgebraException {
this(ba, ma, bg, mg);
this.listener = listener;
}
/**
* Constructor.
*
* @param ba acceleration bias to be set.
* @param ma acceleration cross-coupling errors matrix. Must be 3x3.
* @param bg angular speed bias to be set.
* @param mg angular speed cross-coupling errors matrix. Must be 3x3.
* @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
* @throws AlgebraException if provided cross-coupling matrices cannot
* be inverted.
* @throws IllegalArgumentException if any provided matrices are not 3x3.
*/
public DriftEstimator(
final AccelerationTriad ba,
final Matrix ma,
final AngularSpeedTriad bg,
final Matrix mg,
final Matrix gg) throws AlgebraException {
this(ba, ma, bg, mg);
try {
setAngularSpeedGDependantCrossBias(gg);
} catch (final LockedException ignore) {
// never happens
}
}
/**
* Constructor.
*
* @param ba acceleration bias to be set.
* @param ma acceleration cross-coupling errors matrix. Must be 3x3.
* @param bg angular speed bias to be set.
* @param mg angular speed cross-coupling errors matrix. Must be 3x3.
* @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
* @param listener listener to handle events.
* @throws AlgebraException if provided cross-coupling matrices cannot
* be inverted.
* @throws IllegalArgumentException if any provided matrices are not 3x3.
*/
public DriftEstimator(
final AccelerationTriad ba,
final Matrix ma,
final AngularSpeedTriad bg,
final Matrix mg,
final Matrix gg,
final DriftEstimatorListener listener) throws AlgebraException {
this(ba, ma, bg, mg, gg);
this.listener = listener;
}
/**
* Constructor.
*
* @param ba acceleration bias to be set expressed in meters per squared second
* (m/s`2). Must be 3x1.
* @param ma acceleration cross-coupling errors matrix. Must be 3x3.
* @param bg angular speed bias to be set expressed in radians per second
* (rad/s). Must be 3x1.
* @param mg angular speed cross-coupling errors matrix. Must be 3x3.
* @throws AlgebraException if provided cross-coupling matrices cannot
* be inverted.
* @throws IllegalArgumentException if any provided matrices are not 3x3.
*/
public DriftEstimator(
final Matrix ba,
final Matrix ma,
final Matrix bg,
final Matrix mg) throws AlgebraException {
try {
setAccelerationBias(ba);
setAccelerationCrossCouplingErrors(ma);
setAngularSpeedBias(bg);
setAngularSpeedCrossCouplingErrors(mg);
} catch (final LockedException ignore) {
// never happens
}
}
/**
* Constructor.
*
* @param ba acceleration bias to be set expressed in meters per squared second
* (m/s`2). Must be 3x1.
* @param ma acceleration cross-coupling errors matrix. Must be 3x3.
* @param bg angular speed bias to be set expressed in radians per second
* (rad/s). Must be 3x1.
* @param mg angular speed cross-coupling errors matrix. Must be 3x3.
* @param listener listener to handle events.
* @throws AlgebraException if provided cross-coupling matrices cannot
* be inverted.
* @throws IllegalArgumentException if any provided matrices are not 3x3.
*/
public DriftEstimator(
final Matrix ba,
final Matrix ma,
final Matrix bg,
final Matrix mg,
final DriftEstimatorListener listener) throws AlgebraException {
this(ba, ma, bg, mg);
this.listener = listener;
}
/**
* Constructor.
*
* @param ba acceleration bias to be set expressed in meters per squared second
* (m/s`2). Must be 3x1.
* @param ma acceleration cross-coupling errors matrix. Must be 3x3.
* @param bg angular speed bias to be set expressed in radians per second
* (rad/s). Must be 3x1.
* @param mg angular speed cross-coupling errors matrix. Must be 3x3.
* @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
* @throws AlgebraException if provided cross-coupling matrices cannot
* be inverted.
* @throws IllegalArgumentException if any provided matrices are not 3x3.
*/
public DriftEstimator(
final Matrix ba,
final Matrix ma,
final Matrix bg,
final Matrix mg,
final Matrix gg) throws AlgebraException {
this(ba, ma, bg, mg);
try {
setAngularSpeedGDependantCrossBias(gg);
} catch (final LockedException ignore) {
// never happens
}
}
/**
* Constructor.
*
* @param ba acceleration bias to be set expressed in meters per squared second
* (m/s`2). Must be 3x1.
* @param ma acceleration cross-coupling errors matrix. Must be 3x3.
* @param bg angular speed bias to be set expressed in radians per second
* (rad/s). Must be 3x1.
* @param mg angular speed cross-coupling errors matrix. Must be 3x3.
* @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
* @param listener listener to handle events.
* @throws AlgebraException if provided cross-coupling matrices cannot
* be inverted.
* @throws IllegalArgumentException if any provided matrices are not 3x3.
*/
public DriftEstimator(
final Matrix ba,
final Matrix ma,
final Matrix bg,
final Matrix mg,
final Matrix gg,
final DriftEstimatorListener listener) throws AlgebraException {
this(ba, ma, bg, mg, gg);
this.listener = listener;
}
/**
* Constructor.
*
* @param referenceFrame initial frame containing body position, velocity and
* orientation expressed in ECEF coordinates.
* @param ba acceleration bias to be set.
* @param ma acceleration cross-coupling errors matrix. Must be 3x3.
* @param bg angular speed bias to be set.
* @param mg angular speed cross-coupling errors matrix. Must be 3x3.
* @throws AlgebraException if provided cross-coupling matrices cannot
* be inverted.
* @throws IllegalArgumentException if any provided matrices are not 3x3.
*/
public DriftEstimator(final ECEFFrame referenceFrame,
final AccelerationTriad ba,
final Matrix ma,
final AngularSpeedTriad bg,
final Matrix mg) throws AlgebraException {
this(ba, ma, bg, mg);
this.referenceFrame = referenceFrame;
}
/**
* Constructor.
*
* @param referenceFrame initial frame containing body position, velocity and
* orientation expressed in ECEF coordinates.
* @param ba acceleration bias to be set.
* @param ma acceleration cross-coupling errors matrix. Must be 3x3.
* @param bg angular speed bias to be set.
* @param mg angular speed cross-coupling errors matrix. Must be 3x3.
* @param listener listener to handle events.
* @throws AlgebraException if provided cross-coupling matrices cannot
* be inverted.
* @throws IllegalArgumentException if any provided matrices are not 3x3.
*/
public DriftEstimator(
final ECEFFrame referenceFrame,
final AccelerationTriad ba,
final Matrix ma,
final AngularSpeedTriad bg,
final Matrix mg,
final DriftEstimatorListener listener) throws AlgebraException {
this(ba, ma, bg, mg, listener);
this.referenceFrame = referenceFrame;
}
/**
* Constructor.
*
* @param referenceFrame initial frame containing body position, velocity and
* orientation expressed in ECEF coordinates.
* @param ba acceleration bias to be set.
* @param ma acceleration cross-coupling errors matrix. Must be 3x3.
* @param bg angular speed bias to be set.
* @param mg angular speed cross-coupling errors matrix. Must be 3x3.
* @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
* @throws AlgebraException if provided cross-coupling matrices cannot
* be inverted.
* @throws IllegalArgumentException if any provided matrices are not 3x3.
*/
public DriftEstimator(
final ECEFFrame referenceFrame,
final AccelerationTriad ba,
final Matrix ma,
final AngularSpeedTriad bg,
final Matrix mg,
final Matrix gg) throws AlgebraException {
this(ba, ma, bg, mg, gg);
this.referenceFrame = referenceFrame;
}
/**
* Constructor.
*
* @param referenceFrame initial frame containing body position, velocity and
* orientation expressed in ECEF coordinates.
* @param ba acceleration bias to be set.
* @param ma acceleration cross-coupling errors matrix. Must be 3x3.
* @param bg angular speed bias to be set.
* @param mg angular speed cross-coupling errors matrix. Must be 3x3.
* @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
* @param listener listener to handle events.
* @throws AlgebraException if provided cross-coupling matrices cannot
* be inverted.
* @throws IllegalArgumentException if any provided matrices are not 3x3.
*/
public DriftEstimator(
final ECEFFrame referenceFrame,
final AccelerationTriad ba,
final Matrix ma,
final AngularSpeedTriad bg,
final Matrix mg,
final Matrix gg,
final DriftEstimatorListener listener) throws AlgebraException {
this(ba, ma, bg, mg, gg, listener);
this.referenceFrame = referenceFrame;
}
/**
* Constructor.
*
* @param referenceFrame initial frame containing body position, velocity and
* orientation expressed in ECEF coordinates.
* @param ba acceleration bias to be set expressed in meters per squared second
* (m/s`2). Must be 3x1.
* @param ma acceleration cross-coupling errors matrix. Must be 3x3.
* @param bg angular speed bias to be set expressed in radians per second
* (rad/s). Must be 3x1.
* @param mg angular speed cross-coupling errors matrix. Must be 3x3.
* @throws AlgebraException if provided cross-coupling matrices cannot
* be inverted.
* @throws IllegalArgumentException if any provided matrices are not 3x3.
*/
public DriftEstimator(
final ECEFFrame referenceFrame,
final Matrix ba,
final Matrix ma,
final Matrix bg,
final Matrix mg) throws AlgebraException {
this(ba, ma, bg, mg);
this.referenceFrame = referenceFrame;
}
/**
* Constructor.
*
* @param referenceFrame initial frame containing body position, velocity and
* orientation expressed in ECEF coordinates.
* @param ba acceleration bias to be set expressed in meters per squared second
* (m/s`2). Must be 3x1.
* @param ma acceleration cross-coupling errors matrix. Must be 3x3.
* @param bg angular speed bias to be set expressed in radians per second
* (rad/s). Must be 3x1.
* @param mg angular speed cross-coupling errors matrix. Must be 3x3.
* @param listener listener to handle events.
* @throws AlgebraException if provided cross-coupling matrices cannot
* be inverted.
* @throws IllegalArgumentException if any provided matrices are not 3x3.
*/
public DriftEstimator(
final ECEFFrame referenceFrame,
final Matrix ba,
final Matrix ma,
final Matrix bg,
final Matrix mg,
final DriftEstimatorListener listener) throws AlgebraException {
this(ba, ma, bg, mg, listener);
this.referenceFrame = referenceFrame;
}
/**
* Constructor.
*
* @param referenceFrame initial frame containing body position, velocity and
* orientation expressed in ECEF coordinates.
* @param ba acceleration bias to be set expressed in meters per squared second
* (m/s`2). Must be 3x1.
* @param ma acceleration cross-coupling errors matrix. Must be 3x3.
* @param bg angular speed bias to be set expressed in radians per second
* (rad/s). Must be 3x1.
* @param mg angular speed cross-coupling errors matrix. Must be 3x3.
* @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
* @throws AlgebraException if provided cross-coupling matrices cannot
* be inverted.
* @throws IllegalArgumentException if any provided matrices are not 3x3.
*/
public DriftEstimator(
final ECEFFrame referenceFrame,
final Matrix ba,
final Matrix ma,
final Matrix bg,
final Matrix mg,
final Matrix gg) throws AlgebraException {
this(ba, ma, bg, mg, gg);
this.referenceFrame = referenceFrame;
}
/**
* Constructor.
*
* @param referenceFrame initial frame containing body position, velocity and
* orientation expressed in ECEF coordinates.
* @param ba acceleration bias to be set expressed in meters per squared second
* (m/s`2). Must be 3x1.
* @param ma acceleration cross-coupling errors matrix. Must be 3x3.
* @param bg angular speed bias to be set expressed in radians per second
* (rad/s). Must be 3x1.
* @param mg angular speed cross-coupling errors matrix. Must be 3x3.
* @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
* @param listener listener to handle events.
* @throws AlgebraException if provided cross-coupling matrices cannot
* be inverted.
* @throws IllegalArgumentException if any provided matrices are not 3x3.
*/
public DriftEstimator(
final ECEFFrame referenceFrame,
final Matrix ba,
final Matrix ma,
final Matrix bg,
final Matrix mg,
final Matrix gg,
final DriftEstimatorListener listener) throws AlgebraException {
this(ba, ma, bg, mg, gg, listener);
this.referenceFrame = referenceFrame;
}
/**
* Constructor.
*
* @param referenceFrame initial frame containing body position, velocity and
* orientation expressed in NED coordinates.
* @param ba acceleration bias to be set.
* @param ma acceleration cross-coupling errors matrix. Must be 3x3.
* @param bg angular speed bias to be set.
* @param mg angular speed cross-coupling errors matrix. Must be 3x3.
* @throws AlgebraException if provided cross-coupling matrices cannot
* be inverted.
* @throws IllegalArgumentException if any provided matrices are not 3x3.
*/
public DriftEstimator(final NEDFrame referenceFrame,
final AccelerationTriad ba,
final Matrix ma,
final AngularSpeedTriad bg,
final Matrix mg) throws AlgebraException {
this(NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(referenceFrame), ba, ma, bg, mg);
}
/**
* Constructor.
*
* @param referenceFrame initial frame containing body position, velocity and
* orientation expressed in NED coordinates.
* @param ba acceleration bias to be set.
* @param ma acceleration cross-coupling errors matrix. Must be 3x3.
* @param bg angular speed bias to be set.
* @param mg angular speed cross-coupling errors matrix. Must be 3x3.
* @param listener listener to handle events.
* @throws AlgebraException if provided cross-coupling matrices cannot
* be inverted.
* @throws IllegalArgumentException if any provided matrices are not 3x3.
*/
public DriftEstimator(
final NEDFrame referenceFrame,
final AccelerationTriad ba,
final Matrix ma,
final AngularSpeedTriad bg,
final Matrix mg,
final DriftEstimatorListener listener) throws AlgebraException {
this(NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(referenceFrame), ba, ma, bg, mg, listener);
}
/**
* Constructor.
*
* @param referenceFrame initial frame containing body position, velocity and
* orientation expressed in NED coordinates.
* @param ba acceleration bias to be set.
* @param ma acceleration cross-coupling errors matrix. Must be 3x3.
* @param bg angular speed bias to be set.
* @param mg angular speed cross-coupling errors matrix. Must be 3x3.
* @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
* @throws AlgebraException if provided cross-coupling matrices cannot
* be inverted.
* @throws IllegalArgumentException if any provided matrices are not 3x3.
*/
public DriftEstimator(
final NEDFrame referenceFrame,
final AccelerationTriad ba,
final Matrix ma,
final AngularSpeedTriad bg,
final Matrix mg,
final Matrix gg) throws AlgebraException {
this(NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(referenceFrame), ba, ma, bg, mg, gg);
}
/**
* Constructor.
*
* @param referenceFrame initial frame containing body position, velocity and
* orientation expressed in NED coordinates.
* @param ba acceleration bias to be set.
* @param ma acceleration cross-coupling errors matrix. Must be 3x3.
* @param bg angular speed bias to be set.
* @param mg angular speed cross-coupling errors matrix. Must be 3x3.
* @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
* @param listener listener to handle events.
* @throws AlgebraException if provided cross-coupling matrices cannot
* be inverted.
* @throws IllegalArgumentException if any provided matrices are not 3x3.
*/
public DriftEstimator(
final NEDFrame referenceFrame,
final AccelerationTriad ba,
final Matrix ma,
final AngularSpeedTriad bg,
final Matrix mg,
final Matrix gg,
final DriftEstimatorListener listener) throws AlgebraException {
this(NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(referenceFrame), ba, ma, bg, mg, gg, listener);
}
/**
* Constructor.
*
* @param referenceFrame initial frame containing body position, velocity and
* orientation expressed in NED coordinates.
* @param ba acceleration bias to be set expressed in meters per squared second
* (m/s`2). Must be 3x1.
* @param ma acceleration cross-coupling errors matrix. Must be 3x3.
* @param bg angular speed bias to be set expressed in radians per second
* (rad/s). Must be 3x1.
* @param mg angular speed cross-coupling errors matrix. Must be 3x3.
* @throws AlgebraException if provided cross-coupling matrices cannot
* be inverted.
* @throws IllegalArgumentException if any provided matrices are not 3x3.
*/
public DriftEstimator(
final NEDFrame referenceFrame,
final Matrix ba,
final Matrix ma,
final Matrix bg,
final Matrix mg) throws AlgebraException {
this(NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(referenceFrame), ba, ma, bg, mg);
}
/**
* Constructor.
*
* @param referenceFrame initial frame containing body position, velocity and
* orientation expressed in NED coordinates.
* @param ba acceleration bias to be set expressed in meters per squared second
* (m/s`2). Must be 3x1.
* @param ma acceleration cross-coupling errors matrix. Must be 3x3.
* @param bg angular speed bias to be set expressed in radians per second
* (rad/s). Must be 3x1.
* @param mg angular speed cross-coupling errors matrix. Must be 3x3.
* @param listener listener to handle events.
* @throws AlgebraException if provided cross-coupling matrices cannot
* be inverted.
* @throws IllegalArgumentException if any provided matrices are not 3x3.
*/
public DriftEstimator(
final NEDFrame referenceFrame,
final Matrix ba,
final Matrix ma,
final Matrix bg,
final Matrix mg,
final DriftEstimatorListener listener) throws AlgebraException {
this(NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(referenceFrame), ba, ma, bg, mg, listener);
}
/**
* Constructor.
*
* @param referenceFrame initial frame containing body position, velocity and
* orientation expressed in NED coordinates.
* @param ba acceleration bias to be set expressed in meters per squared second
* (m/s`2). Must be 3x1.
* @param ma acceleration cross-coupling errors matrix. Must be 3x3.
* @param bg angular speed bias to be set expressed in radians per second
* (rad/s). Must be 3x1.
* @param mg angular speed cross-coupling errors matrix. Must be 3x3.
* @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
* @throws AlgebraException if provided cross-coupling matrices cannot
* be inverted.
* @throws IllegalArgumentException if any provided matrices are not 3x3.
*/
public DriftEstimator(
final NEDFrame referenceFrame,
final Matrix ba,
final Matrix ma,
final Matrix bg,
final Matrix mg,
final Matrix gg) throws AlgebraException {
this(NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(referenceFrame), ba, ma, bg, mg, gg);
}
/**
* Constructor.
*
* @param referenceFrame initial frame containing body position, velocity and
* orientation expressed in ECEF coordinates.
* @param ba acceleration bias to be set expressed in meters per squared second
* (m/s`2). Must be 3x1.
* @param ma acceleration cross-coupling errors matrix. Must be 3x3.
* @param bg angular speed bias to be set expressed in radians per second
* (rad/s). Must be 3x1.
* @param mg angular speed cross-coupling errors matrix. Must be 3x3.
* @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
* @param listener listener to handle events.
* @throws AlgebraException if provided cross-coupling matrices cannot
* be inverted.
* @throws IllegalArgumentException if any provided matrices are not 3x3.
*/
public DriftEstimator(
final NEDFrame referenceFrame,
final Matrix ba,
final Matrix ma,
final Matrix bg,
final Matrix mg,
final Matrix gg,
final DriftEstimatorListener listener) throws AlgebraException {
this(NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(referenceFrame), ba, ma, bg, mg, gg, listener);
}
/**
* Gets listener to handle events raised by this estimator.
*
* @return listener to handle events raised by this estimator.
*/
public DriftEstimatorListener getListener() {
return listener;
}
/**
* Sets listener to handle events raised by this estimator.
*
* @param listener listener to handle events raised by this estimator.
* @throws LockedException if estimator is running.
*/
public void setListener(final DriftEstimatorListener listener) throws LockedException {
if (running) {
throw new LockedException();
}
this.listener = listener;
}
/**
* Gets initial frame containing body position, velocity and orientation
* expressed in ECEF coordinates before starting drift estimation.
*
* @return initial body frame or null.
*/
public ECEFFrame getReferenceFrame() {
return referenceFrame;
}
/**
* Sets initial frame containing body position, velocity and orientation
* expressed in ECEF coordinates before starting drift estimation.
*
* @param referenceFrame initial frame or null.
* @throws LockedException if estimator is already running.
*/
public void setReferenceFrame(final ECEFFrame referenceFrame) throws LockedException {
if (running) {
throw new LockedException();
}
this.referenceFrame = referenceFrame;
}
/**
* Gets initial frame containing body position, velocity and orientation
* expressed in NED coordinates before starting drift estimation.
*
* @return initial body frame or null.
*/
public NEDFrame getReferenceNedFrame() {
return referenceFrame != null
? ECEFtoNEDFrameConverter.convertECEFtoNEDAndReturnNew(referenceFrame)
: null;
}
/**
* Gets initial frame containing body position, velocity and orientation
* expressed in NED coordinates before starting drift estimation.
*
* @param result instance where the result will be stored.
* @return true if the initial frame was available and the result was updated, false
* otherwise.
* @throws NullPointerException if provided result instance is null.
*/
public boolean getReferenceNedFrame(final NEDFrame result) {
if (referenceFrame != null) {
ECEFtoNEDFrameConverter.convertECEFtoNED(referenceFrame, result);
return true;
} else {
return false;
}
}
/**
* Sets an initial frame containing body position, velocity and orientation
* expressed in NED coordinates before starting drift estimation.
*
* @param referenceNedFrame initial body frame or null.
* @throws LockedException if estimator is already running.
*/
public void setReferenceNedFrame(final NEDFrame referenceNedFrame) throws LockedException {
if (running) {
throw new LockedException();
}
if (referenceNedFrame != null) {
if (referenceFrame != null) {
NEDtoECEFFrameConverter.convertNEDtoECEF(referenceNedFrame, referenceFrame);
} else {
referenceFrame = NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(referenceNedFrame);
}
} else {
referenceFrame = null;
}
}
/**
* Gets initial body position, expressed in ECEF coordinates, before starting
* drift estimation.
*
* @return initial body position or null.
*/
public ECEFPosition getReferenceEcefPosition() {
return referenceFrame != null ? referenceFrame.getECEFPosition() : null;
}
/**
* Gets initial body position, expressed in ECEF coordinates, before starting
* drift estimation.
*
* @param result instance where the result will be stored.
* @return true if the initial body position was available and the result was updated,
* false otherwise.
* @throws NullPointerException if provided result instance is null.
*/
public boolean getReferenceEcefPosition(final ECEFPosition result) {
if (referenceFrame != null) {
referenceFrame.getECEFPosition(result);
return true;
} else {
return false;
}
}
/**
* Sets initial body position, expressed in ECEF coordinates, before starting
* drift estimation.
*
* @param referenceEcefPosition initial body position.
* @throws LockedException if estimator is already running.
* @throws NullPointerException if provided position is null.
*/
public void setReferenceEcefPosition(final ECEFPosition referenceEcefPosition) throws LockedException {
if (running) {
throw new LockedException();
}
if (referenceFrame != null) {
referenceFrame.setPosition(referenceEcefPosition);
} else {
referenceFrame = new ECEFFrame(referenceEcefPosition);
}
}
/**
* Gets initial body velocity, expressed in ECEF coordinates, before starting
* drift estimation.
*
* @return initial body velocity or null.
*/
public ECEFVelocity getReferenceEcefVelocity() {
return referenceFrame != null ? referenceFrame.getECEFVelocity() : null;
}
/**
* Gets initial body velocity, expressed in ECEF coordinates, before starting
* drift estimation.
*
* @param result instance where the result will be stored.
* @return true if initial body velocity was available and the result was updated,
* false otherwise.
* @throws NullPointerException if provided result instance is null.
*/
public boolean getReferenceEcefVelocity(final ECEFVelocity result) {
if (referenceFrame != null) {
referenceFrame.getECEFVelocity(result);
return true;
} else {
return false;
}
}
/**
* Sets initial body velocity, expressed in ECEF coordinates, before starting
* drift estimation.
*
* @param referenceEcefVelocity initial body velocity.
* @throws LockedException if estimator is already running.
* @throws NullPointerException if velocity is null.
*/
public void setReferenceEcefVelocity(final ECEFVelocity referenceEcefVelocity) throws LockedException {
if (running) {
throw new LockedException();
}
if (referenceFrame == null) {
referenceFrame = new ECEFFrame();
}
referenceFrame.setVelocity(referenceEcefVelocity);
}
/**
* Gets initial body coordinate transformation, containing body orientation
* expressed in ECEF coordinates, before starting estimation.
*
* @return initial body orientation or null.
*/
public CoordinateTransformation getReferenceEcefCoordinateTransformation() {
return referenceFrame != null ?
referenceFrame.getCoordinateTransformation() : null;
}
/**
* Gets initial body coordinate transformation, containing body orientation
* expressed in ECEF coordinates, before starting estimation.
*
* @param result instance where the result will be stored.
* @return true if initial body orientation was available and a result was
* updated, false otherwise.
* @throws NullPointerException if provided result instance is null.
*/
public boolean getReferenceEcefCoordinateTransformation(final CoordinateTransformation result) {
if (referenceFrame != null) {
referenceFrame.getCoordinateTransformation(result);
return true;
} else {
return false;
}
}
/**
* Sets initial body coordinate transformation, containing body orientation
* expressed in ECEF coordinates, before starting estimation.
*
* @param referenceEcefCoordinateTransformation initial body orientation.
* @throws LockedException if estimator is already running.
* @throws InvalidSourceAndDestinationFrameTypeException if source and
* destination types are invalid. Source type must be
* {@link com.irurueta.navigation.frames.FrameType#BODY_FRAME} and the destination
* type must be {@link com.irurueta.navigation.frames.FrameType#EARTH_CENTERED_EARTH_FIXED_FRAME}
* indicating that body orientation is expressed respect ECEF coordinates.
* @throws NullPointerException if orientation is null.
*/
public void setReferenceEcefCoordinateTransformation(
final CoordinateTransformation referenceEcefCoordinateTransformation) throws LockedException,
InvalidSourceAndDestinationFrameTypeException {
if (running) {
throw new LockedException();
}
if (referenceFrame == null) {
referenceFrame = new ECEFFrame(referenceEcefCoordinateTransformation);
} else {
referenceFrame.setCoordinateTransformation(referenceEcefCoordinateTransformation);
}
}
/**
* Gets initial body position, expressed in NED coordinates, before starting
* drift estimation.
*
* @return initial body position or null.
*/
public NEDPosition getReferenceNedPosition() {
final var nedFrame = getReferenceNedFrame();
return nedFrame != null ? nedFrame.getPosition() : null;
}
/**
* Gets initial body position, expressed in NED coordinates, before starting
* drift estimation.
*
* @param result instance where the result will be stored.
* @return true if the initial body position was available and the result was updated,
* false otherwise.
* @throws NullPointerException if provided result instance is null.
*/
public boolean getReferenceNedPosition(final NEDPosition result) {
if (referenceFrame != null) {
final var nedFrame = getReferenceNedFrame();
nedFrame.getPosition(result);
return true;
} else {
return false;
}
}
/**
* Sets initial body position, expressed in NED coordinates, before starting
* drift estimation.
*
* @param referenceNedPosition initial body position.
* @throws LockedException if estimator is already running.
* @throws NullPointerException if provided position is null.
*/
public void setReferenceNedPosition(final NEDPosition referenceNedPosition) throws LockedException {
if (running) {
throw new LockedException();
}
if (referenceFrame != null) {
final var nedFrame = getReferenceNedFrame();
nedFrame.setPosition(referenceNedPosition);
setReferenceNedFrame(nedFrame);
} else {
setReferenceNedFrame(new NEDFrame(referenceNedPosition));
}
}
/**
* Gets initial body velocity, expressed in NED coordinates, before starting
* drift estimation.
*
* @return initial body velocity or null.
*/
public NEDVelocity getReferenceNedVelocity() {
final var nedFrame = getReferenceNedFrame();
return nedFrame != null ? nedFrame.getVelocity() : null;
}
/**
* Gets initial body velocity, expressed in NED coordinates, before starting
* drift estimation.
*
* @param result instance where the result will be stored.
* @return true if initial body velocity was available and the result was updated,
* false otherwise.
* @throws NullPointerException if provided result instance is null.
*/
public boolean getReferenceNedVelocity(final NEDVelocity result) {
if (referenceFrame != null) {
final var nedFrame = getReferenceNedFrame();
nedFrame.getVelocity(result);
return true;
} else {
return false;
}
}
/**
* Sets initial body velocity, expressed in NED coordinates, before starting
* drift estimation.
*
* @param referenceNedVelocity initial body velocity.
* @throws LockedException if estimator is already running.
* @throws NullPointerException if velocity is null.
*/
public void setReferenceNedVelocity(final NEDVelocity referenceNedVelocity) throws LockedException {
if (running) {
throw new LockedException();
}
final NEDFrame nedFrame;
if (referenceFrame != null) {
nedFrame = getReferenceNedFrame();
} else {
nedFrame = new NEDFrame();
}
nedFrame.setVelocity(referenceNedVelocity);
setReferenceNedFrame(nedFrame);
}
/**
* Gets initial body coordinate transformation, containing body orientation
* expressed in NED coordinates, before starting estimation.
*
* @return initial body orientation or null.
*/
public CoordinateTransformation getReferenceNedCoordinateTransformation() {
final var nedFrame = getReferenceNedFrame();
return nedFrame != null ? nedFrame.getCoordinateTransformation() : null;
}
/**
* Gets initial body coordinate transformation, containing body orientation
* expressed in NED coordinates, before starting estimation.
*
* @param result instance where the result will be stored.
* @return true if initial body orientation was available and the result was
* updated, false otherwise.
* @throws NullPointerException if provided result instance is null.
*/
public boolean getReferenceNedCoordinateTransformation(final CoordinateTransformation result) {
if (referenceFrame != null) {
final var nedFrame = getReferenceNedFrame();
nedFrame.getCoordinateTransformation(result);
return true;
} else {
return false;
}
}
/**
* Sets initial body coordinate transformation, containing body orientation
* expressed in NED coordinates, before starting estimation.
*
* @param referenceNedCoordinateTransformation initial body orientation.
* @throws LockedException if estimator is already running.
* @throws InvalidSourceAndDestinationFrameTypeException if source and
* destination types are invalid. Source type must be
* {@link com.irurueta.navigation.frames.FrameType#BODY_FRAME} and the destination
* type must be {@link com.irurueta.navigation.frames.FrameType#LOCAL_NAVIGATION_FRAME}
* indicating that body orientation is expressed respect NED coordinates.
* @throws NullPointerException if orientation is null.
*/
public void setReferenceNedCoordinateTransformation(
final CoordinateTransformation referenceNedCoordinateTransformation) throws LockedException,
InvalidSourceAndDestinationFrameTypeException {
if (running) {
throw new LockedException();
}
final NEDFrame nedFrame;
if (referenceFrame == null) {
nedFrame = new NEDFrame(referenceNedCoordinateTransformation);
setReferenceNedFrame(nedFrame);
} else {
nedFrame = getReferenceNedFrame();
nedFrame.setCoordinateTransformation(referenceNedCoordinateTransformation);
}
}
/**
* Gets acceleration bias values expressed in meters per squared second (m/s^2).
*
* @return bias values expressed in meters per squared second.
*/
public Matrix getAccelerationBias() {
return fixer.getAccelerationBias();
}
/**
* Gets acceleration bias values expressed in meters per squared second (m/s^2).
*
* @param result instance where the result will be stored.
*/
public void getAccelerationBias(final Matrix result) {
fixer.getAccelerationBias(result);
}
/**
* Sets acceleration bias values expressed in meters per squared second (m/s^2).
*
* @param bias bias values expressed in meters per squared second.
* Must be 3x1.
* @throws LockedException if estimator is running.
* @throws IllegalArgumentException if any provided matrix is not 3x1.
*/
public void setAccelerationBias(final Matrix bias) throws LockedException {
if (running) {
throw new LockedException();
}
fixer.setAccelerationBias(bias);
}
/**
* Gets acceleration bias values expressed in meters per squared second (m/s^2).
*
* @return bias values expressed in meters per squared second.
*/
public double[] getAccelerationBiasArray() {
return fixer.getAccelerationBiasArray();
}
/**
* Gets acceleration bias values expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be stored.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
public void getAccelerationBiasArray(final double[] result) {
fixer.getAccelerationBiasArray(result);
}
/**
* Sets acceleration bias values expressed in meters per squared second (m/s^2).
*
* @param bias bias values expressed in meters per squared second (m/s^2).
* Must have length 3.
* @throws IllegalArgumentException if provided array does not have length 3.
* @throws LockedException if estimator is running.
*/
public void setAccelerationBias(final double[] bias) throws LockedException {
if (running) {
throw new LockedException();
}
fixer.setAccelerationBias(bias);
}
/**
* Gets acceleration bias.
*
* @return acceleration bias.
*/
public AccelerationTriad getAccelerationBiasAsTriad() {
return fixer.getAccelerationBiasAsTriad();
}
/**
* Gets acceleration bias.
*
* @param result instance where the result will be stored.
*/
public void getAccelerationBiasAsTriad(final AccelerationTriad result) {
fixer.getAccelerationBiasAsTriad(result);
}
/**
* Sets acceleration bias.
*
* @param bias acceleration bias to be set.
* @throws LockedException if estimator is running.
*/
public void setAccelerationBias(final AccelerationTriad bias) throws LockedException {
if (running) {
throw new LockedException();
}
fixer.setAccelerationBias(bias);
}
/**
* Gets acceleration x-coordinate of bias expressed in meters per squared
* second (m/s^2).
*
* @return x-coordinate of bias expressed in meters per squared second (m/s^2).
*/
public double getAccelerationBiasX() {
return fixer.getAccelerationBiasX();
}
/**
* Sets acceleration x-coordinate of bias expressed in meters per squared
* second (m/s^2).
*
* @param biasX x-coordinate of bias expressed in meters per squared second
* (m/s^2).
* @throws LockedException if estimator is running.
*/
public void setAccelerationBiasX(final double biasX) throws LockedException {
if (running) {
throw new LockedException();
}
fixer.setAccelerationBiasX(biasX);
}
/**
* Gets acceleration y-coordinate of bias expressed in meters per squared
* second (m/s^2).
*
* @return y-coordinate of bias expressed in meters per squared second (m/s^2).
*/
public double getAccelerationBiasY() {
return fixer.getAccelerationBiasY();
}
/**
* Sets acceleration y-coordinate of bias expressed in meters per squared
* second (m/s^2).
*
* @param biasY y-coordinate of bias expressed in meters per squared second
* (m/s^2).
* @throws LockedException if estimator is running.
*/
public void setAccelerationBiasY(final double biasY) throws LockedException {
if (running) {
throw new LockedException();
}
fixer.setAccelerationBiasY(biasY);
}
/**
* Gets acceleration z-coordinate of bias expressed in meters per squared
* second (m/s^2).
*
* @return z-coordinate of bias expressed in meters per squared second (m/s^2).
*/
public double getAccelerationBiasZ() {
return fixer.getAccelerationBiasZ();
}
/**
* Sets acceleration z-coordinate of bias expressed in meters per squared
* second (m/s^2).
*
* @param biasZ z-coordinate of bias expressed in meters per squared second (m/s^2).
* @throws LockedException if estimator is running.
*/
public void setAccelerationBiasZ(final double biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
fixer.setAccelerationBiasZ(biasZ);
}
/**
* Sets acceleration coordinates of bias expressed in meters per squared
* second (m/s^2).
*
* @param biasX x-coordinate of bias.
* @param biasY y-coordinate of bias.
* @param biasZ z-coordinate of bias.
* @throws LockedException if estimator is running.
*/
public void setAccelerationBias(final double biasX, final double biasY, final double biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
fixer.setAccelerationBias(biasX, biasY, biasZ);
}
/**
* Gets acceleration x-coordinate of bias.
*
* @return acceleration x-coordinate of bias.
*/
public Acceleration getAccelerationBiasXAsAcceleration() {
return fixer.getAccelerationBiasXAsAcceleration();
}
/**
* Gets acceleration x-coordinate of bias.
*
* @param result instance where the result will be stored.
*/
public void getAccelerationBiasXAsAcceleration(final Acceleration result) {
fixer.getAccelerationBiasXAsAcceleration(result);
}
/**
* Sets acceleration x-coordinate of bias.
*
* @param biasX acceleration x-coordinate of bias.
* @throws LockedException if estimator is running.
*/
public void setAccelerationBiasX(final Acceleration biasX) throws LockedException {
if (running) {
throw new LockedException();
}
fixer.setAccelerationBiasX(biasX);
}
/**
* Gets acceleration y-coordinate of bias.
*
* @return acceleration y-coordinate of bias.
*/
public Acceleration getAccelerationBiasYAsAcceleration() {
return fixer.getAccelerationBiasYAsAcceleration();
}
/**
* Gets acceleration y-coordinate of bias.
*
* @param result instance where the result will be stored.
*/
public void getAccelerationBiasYAsAcceleration(final Acceleration result) {
fixer.getAccelerationBiasYAsAcceleration(result);
}
/**
* Sets acceleration y-coordinate of bias.
*
* @param biasY acceleration y-coordinate of bias.
* @throws LockedException if estimator is running.
*/
public void setAccelerationBiasY(final Acceleration biasY) throws LockedException {
if (running) {
throw new LockedException();
}
fixer.setAccelerationBiasY(biasY);
}
/**
* Gets acceleration z-coordinate of bias.
*
* @return acceleration z-coordinate of bias.
*/
public Acceleration getAccelerationBiasZAsAcceleration() {
return fixer.getAccelerationBiasZAsAcceleration();
}
/**
* Gets acceleration z-coordinate of bias.
*
* @param result instance where the result will be stored.
*/
public void getAccelerationBiasZAsAcceleration(final Acceleration result) {
fixer.getAccelerationBiasZAsAcceleration(result);
}
/**
* Sets acceleration z-coordinate of bias.
*
* @param biasZ z-coordinate of bias.
* @throws LockedException if estimator is running.
*/
public void setAccelerationBiasZ(final Acceleration biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
fixer.setAccelerationBiasZ(biasZ);
}
/**
* Sets acceleration coordinates of bias.
*
* @param biasX x-coordinate of bias.
* @param biasY y-coordinate of bias.
* @param biasZ z-coordinate of bias.
* @throws LockedException if estimator is running.
*/
public void setAccelerationBias(
final Acceleration biasX,
final Acceleration biasY,
final Acceleration biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
fixer.setAccelerationBias(biasX, biasY, biasZ);
}
/**
* Gets acceleration cross-coupling errors matrix.
*
* @return acceleration cross-coupling errors matrix.
*/
public Matrix getAccelerationCrossCouplingErrors() {
return fixer.getAccelerationCrossCouplingErrors();
}
/**
* Gets acceleration cross-coupling errors matrix.
*
* @param result instance where the result will be stored.
*/
public void getAccelerationCrossCouplingErrors(final Matrix result) {
fixer.getAccelerationCrossCouplingErrors(result);
}
/**
* Sets acceleration cross-coupling errors matrix.
*
* @param crossCouplingErrors acceleration cross-coupling errors matrix.
* Must be 3x3.
* @throws LockedException if estimator is running.
* @throws AlgebraException if matrix cannot be inverted.
* @throws IllegalArgumentException if matrix is not 3x3.
*/
public void setAccelerationCrossCouplingErrors(final Matrix crossCouplingErrors) throws AlgebraException,
LockedException {
if (running) {
throw new LockedException();
}
fixer.setAccelerationCrossCouplingErrors(crossCouplingErrors);
}
/**
* Gets acceleration x scaling factor.
*
* @return x scaling factor.
*/
public double getAccelerationSx() {
return fixer.getAccelerationSx();
}
/**
* Sets acceleration x scaling factor.
*
* @param sx x scaling factor.
* @throws LockedException if estimator is running.
* @throws AlgebraException if provided value makes acceleration cross-coupling matrix non-invertible.
*/
public void setAccelerationSx(final double sx) throws LockedException, AlgebraException {
if (running) {
throw new LockedException();
}
fixer.setAccelerationSx(sx);
}
/**
* Gets acceleration y scaling factor.
*
* @return y scaling factor.
*/
public double getAccelerationSy() {
return fixer.getAccelerationSy();
}
/**
* Sets acceleration y scaling factor.
*
* @param sy y scaling factor.
* @throws LockedException if estimator is running.
* @throws AlgebraException if provided value makes acceleration cross-coupling matrix non-invertible.
*/
public void setAccelerationSy(final double sy) throws LockedException, AlgebraException {
if (running) {
throw new LockedException();
}
fixer.setAccelerationSy(sy);
}
/**
* Gets acceleration z scaling factor.
*
* @return z scaling factor.
*/
public double getAccelerationSz() {
return fixer.getAccelerationSz();
}
/**
* Sets acceleration z scaling factor.
*
* @param sz z scaling factor.
* @throws LockedException if estimator is running.
* @throws AlgebraException if provided value makes acceleration cross-coupling matrix non-invertible.
*/
public void setAccelerationSz(final double sz) throws LockedException, AlgebraException {
if (running) {
throw new LockedException();
}
fixer.setAccelerationSz(sz);
}
/**
* Gets acceleration x-y cross-coupling error.
*
* @return acceleration x-y cross-coupling error.
*/
public double getAccelerationMxy() {
return fixer.getAccelerationMxy();
}
/**
* Sets acceleration x-y cross-coupling error.
*
* @param mxy acceleration x-y cross-coupling error.
* @throws LockedException if estimator is running.
* @throws AlgebraException if provided value makes acceleration cross-coupling matrix non-invertible.
*/
public void setAccelerationMxy(final double mxy) throws LockedException, AlgebraException {
if (running) {
throw new LockedException();
}
fixer.setAccelerationMxy(mxy);
}
/**
* Gets acceleration x-z cross-coupling error.
*
* @return acceleration x-z cross-coupling error.
*/
public double getAccelerationMxz() {
return fixer.getAccelerationMxz();
}
/**
* Sets acceleration x-z cross-coupling error.
*
* @param mxz acceleration x-z cross-coupling error.
* @throws LockedException if estimator is running.
* @throws AlgebraException if provided value makes acceleration cross-coupling matrix non-invertible.
*/
public void setAccelerationMxz(final double mxz) throws LockedException, AlgebraException {
if (running) {
throw new LockedException();
}
fixer.setAccelerationMxz(mxz);
}
/**
* Gets acceleration y-x cross-coupling error.
*
* @return acceleration y-x cross-coupling error.
*/
public double getAccelerationMyx() {
return fixer.getAccelerationMyx();
}
/**
* Sets acceleration y-x cross-coupling error.
*
* @param myx acceleration y-x cross-coupling error.
* @throws LockedException if estimator is running.
* @throws AlgebraException if provided value makes acceleration cross-coupling matrix non-invertible.
*/
public void setAccelerationMyx(final double myx) throws LockedException, AlgebraException {
if (running) {
throw new LockedException();
}
fixer.setAccelerationMyx(myx);
}
/**
* Gets acceleration y-z cross-coupling error.
*
* @return y-z cross coupling error.
*/
public double getAccelerationMyz() {
return fixer.getAccelerationMyz();
}
/**
* Sets acceleration y-z cross-coupling error.
*
* @param myz y-z cross coupling error.
* @throws LockedException if estimator is running.
* @throws AlgebraException if provided value makes acceleration cross-coupling matrix non-invertible.
*/
public void setAccelerationMyz(final double myz) throws LockedException, AlgebraException {
if (running) {
throw new LockedException();
}
fixer.setAccelerationMyz(myz);
}
/**
* Gets acceleration z-x cross-coupling error.
*
* @return acceleration z-x cross-coupling error.
*/
public double getAccelerationMzx() {
return fixer.getAccelerationMzx();
}
/**
* Sets acceleration z-x cross coupling error.
*
* @param mzx acceleration z-x cross coupling error.
* @throws LockedException if estimator is running.
* @throws AlgebraException if provided value makes acceleration cross-coupling matrix non-invertible.
*/
public void setAccelerationMzx(final double mzx) throws LockedException, AlgebraException {
if (running) {
throw new LockedException();
}
fixer.setAccelerationMzx(mzx);
}
/**
* Gets acceleration z-y cross-coupling error.
*
* @return acceleration z-y cross-coupling error.
*/
public double getAccelerationMzy() {
return fixer.getAccelerationMzy();
}
/**
* Sets acceleration z-y cross-coupling error.
*
* @param mzy acceleration z-y cross coupling error.
* @throws LockedException if estimator is running.
* @throws AlgebraException if provided value makes acceleration cross-coupling matrix non-invertible.
*/
public void setAccelerationMzy(final double mzy) throws LockedException, AlgebraException {
if (running) {
throw new LockedException();
}
fixer.setAccelerationMzy(mzy);
}
/**
* Sets acceleration scaling factors.
*
* @param sx x scaling factor.
* @param sy y scaling factor.
* @param sz z scaling factor.
* @throws LockedException if estimator is running.
* @throws AlgebraException if provided values make acceleration cross-coupling matrix non-invertible.
*/
public void setAccelerationScalingFactors(final double sx, final double sy, final double sz)
throws LockedException, AlgebraException {
if (running) {
throw new LockedException();
}
fixer.setAccelerationScalingFactors(sx, sy, sz);
}
/**
* Sets acceleration cross-coupling errors.
*
* @param mxy x-y cross coupling error.
* @param mxz x-z cross coupling error.
* @param myx y-x cross coupling error.
* @param myz y-z cross coupling error.
* @param mzx z-x cross coupling error.
* @param mzy z-y cross coupling error.
* @throws LockedException if estimator is running.
* @throws AlgebraException if provided values make acceleration cross-coupling matrix non-invertible.
*/
public void setAccelerationCrossCouplingErrors(
final double mxy, final double mxz,
final double myx, final double myz,
final double mzx, final double mzy) throws LockedException, AlgebraException {
if (running) {
throw new LockedException();
}
fixer.setAccelerationCrossCouplingErrors(mxy, mxz, myx, myz, mzx, mzy);
}
/**
* Sets acceleration scaling factors and cross-coupling errors.
*
* @param sx x scaling factor.
* @param sy y scaling factor.
* @param sz z scaling factor.
* @param mxy x-y cross coupling error.
* @param mxz x-z cross coupling error.
* @param myx y-x cross coupling error.
* @param myz y-z cross coupling error.
* @param mzx z-x cross coupling error.
* @param mzy z-y cross coupling error.
* @throws LockedException if estimator is running.
* @throws AlgebraException if provided values make acceleration cross-coupling matrix non-invertible.
*/
public void setAccelerationScalingFactorsAndCrossCouplingErrors(
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 LockedException, AlgebraException {
if (running) {
throw new LockedException();
}
fixer.setAccelerationScalingFactorsAndCrossCouplingErrors(sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
}
/**
* Gets angular speed bias values expressed in radians per second (rad/s).
*
* @return angular speed bias values expressed in radians per second.
*/
public Matrix getAngularSpeedBias() {
return fixer.getAngularSpeedBias();
}
/**
* Gets angular speed bias values expressed in radians per second (rad/s).
*
* @param result instance where the result will be stored.
*/
public void getAngularSpeedBias(final Matrix result) {
fixer.getAngularSpeedBias(result);
}
/**
* Sets angular speed bias values expressed in radians per second (rad/s).
*
* @param bias bias values expressed in radians per second. Must be 3x1.
* @throws IllegalArgumentException if matrix is not 3x1.
* @throws LockedException if estimator is running.
*/
public void setAngularSpeedBias(final Matrix bias) throws LockedException {
if (running) {
throw new LockedException();
}
fixer.setAngularSpeedBias(bias);
}
/**
* Gets angular speed bias values expressed in radians per second (rad/s).
*
* @return bias values expressed in radians per second.
*/
public double[] getAngularSpeedBiasArray() {
return fixer.getAngularSpeedBiasArray();
}
/**
* Gets angular speed bias values expressed in radians per second (rad/s).
*
* @param result instance where result data will be stored.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
public void getAngularSpeedBiasArray(final double[] result) {
fixer.getAngularSpeedBiasArray(result);
}
/**
* Sets angular speed bias values expressed in radians per second (rad/s).
*
* @param bias bias values expressed in radians per second (rad/s). Must
* have length 3.
* @throws IllegalArgumentException if provided array does not have length 3.
* @throws LockedException if estimator is running.
*/
public void setAngularSpeedBias(final double[] bias) throws LockedException {
if (running) {
throw new LockedException();
}
fixer.setAngularSpeedBias(bias);
}
/**
* Gets angular speed bias.
*
* @return angular speed bias.
*/
public AngularSpeedTriad getAngularSpeedBiasAsTriad() {
return fixer.getAngularSpeedBiasAsTriad();
}
/**
* Gets angular speed bias.
*
* @param result instance where the result will be stored.
*/
public void getAngularSpeedBiasAsTriad(final AngularSpeedTriad result) {
fixer.getAngularSpeedBiasAsTriad(result);
}
/**
* Sets angular speed bias.
*
* @param bias angular speed bias to be set.
* @throws LockedException if estimator is running.
*/
public void setAngularSpeedBias(final AngularSpeedTriad bias) throws LockedException {
if (running) {
throw new LockedException();
}
fixer.setAngularSpeedBias(bias);
}
/**
* Gets angular speed x-coordinate of bias expressed in radians per second
* (rad/s).
*
* @return x-coordinate of bias expressed in radians per second (rad/s).
*/
public double getAngularSpeedBiasX() {
return fixer.getAngularSpeedBiasX();
}
/**
* Sets angular speed x-coordinate of bias expressed in radians per second
* (rad/s).
*
* @param biasX x-coordinate of bias expressed in radians per second (rad/s).
* @throws LockedException if estimator is running.
*/
public void setAngularSpeedBiasX(final double biasX) throws LockedException {
if (running) {
throw new LockedException();
}
fixer.setAngularSpeedBiasX(biasX);
}
/**
* Gets angular speed y-coordinate of bias expressed in radians per second
* (rad/s).
*
* @return y-coordinate of bias expressed in radians per second (rad/s).
*/
public double getAngularSpeedBiasY() {
return fixer.getAngularSpeedBiasY();
}
/**
* Sets angular speed y-coordinate of bias expressed in radians per second
* (rad/s).
*
* @param biasY y-coordinate of bias expressed in radians per second (rad/s).
* @throws LockedException if estimator is running.
*/
public void setAngularSpeedBiasY(final double biasY) throws LockedException {
if (running) {
throw new LockedException();
}
fixer.setAngularSpeedBiasY(biasY);
}
/**
* Gets angular speed z-coordinate of bias expressed in radians per second
* (rad/s).
*
* @return z-coordinate of bias expressed in radians per second (rad/s).
*/
public double getAngularSpeedBiasZ() {
return fixer.getAngularSpeedBiasZ();
}
/**
* Sets angular speed z-coordinate of bias expressed in radians per second
* (rad/s).
*
* @param biasZ z-coordinate of bias expressed in radians per second (rad/s).
* @throws LockedException if estimator is running.
*/
public void setAngularSpeedBiasZ(final double biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
fixer.setAngularSpeedBiasZ(biasZ);
}
/**
* Sets angular speed coordinates of bias expressed in radians per second
* (rad/s).
*
* @param biasX x-coordinate of bias.
* @param biasY y-coordinate of bias.
* @param biasZ z-coordinate of bias.
* @throws LockedException if estimator is running.
*/
public void setAngularSpeedBias(final double biasX, final double biasY, final double biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
fixer.setAngularSpeedBias(biasX, biasY, biasZ);
}
/**
* Gets angular speed x-coordinate of bias.
*
* @return x-coordinate of bias.
*/
public AngularSpeed getAngularSpeedBiasXAsAngularSpeed() {
return fixer.getAngularSpeedBiasXAsAngularSpeed();
}
/**
* Gets angular speed x-coordinate of bias.
*
* @param result instance where the result will be stored.
*/
public void getAngularSpeedBiasXAsAngularSpeed(final AngularSpeed result) {
fixer.getAngularSpeedBiasXAsAngularSpeed(result);
}
/**
* Sets angular speed x-coordinate of bias.
*
* @param biasX x-coordinate of bias.
* @throws LockedException if estimator is running.
*/
public void setAngularSpeedBiasX(final AngularSpeed biasX) throws LockedException {
if (running) {
throw new LockedException();
}
fixer.setAngularSpeedBiasX(biasX);
}
/**
* Gets angular speed y-coordinate of bias.
*
* @return y-coordinate of bias.
*/
public AngularSpeed getAngularSpeedBiasYAsAngularSpeed() {
return fixer.getAngularSpeedBiasYAsAngularSpeed();
}
/**
* Gets angular speed y-coordinate of bias.
*
* @param result instance where the result will be stored.
*/
public void getAngularSpeedBiasYAsAngularSpeed(final AngularSpeed result) {
fixer.getAngularSpeedBiasYAsAngularSpeed(result);
}
/**
* Sets angular speed y-coordinate of bias.
*
* @param biasY y-coordinate of bias.
* @throws LockedException if estimator is running.
*/
public void setAngularSpeedBiasY(final AngularSpeed biasY) throws LockedException {
if (running) {
throw new LockedException();
}
fixer.setAngularSpeedBiasY(biasY);
}
/**
* Gets angular speed z-coordinate of bias.
*
* @return z-coordinate of bias.
*/
public AngularSpeed getAngularSpeedBiasZAsAngularSpeed() {
return fixer.getAngularSpeedBiasZAsAngularSpeed();
}
/**
* Gets angular speed z-coordinate of bias.
*
* @param result instance where the result will be stored.
*/
public void getAngularSpeedBiasZAsAngularSpeed(final AngularSpeed result) {
fixer.getAngularSpeedBiasZAsAngularSpeed(result);
}
/**
* Sets angular speed z-coordinate of bias.
*
* @param biasZ z-coordinate of bias.
* @throws LockedException if estimator is running.
*/
public void setAngularSpeedBiasZ(final AngularSpeed biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
fixer.setAngularSpeedBiasZ(biasZ);
}
/**
* Sets angular speed coordinates of bias.
*
* @param biasX x-coordinate of bias.
* @param biasY y-coordinate of bias.
* @param biasZ z-coordinate of bias.
* @throws LockedException if estimator is running.
*/
public void setAngularSpeedBias(
final AngularSpeed biasX,
final AngularSpeed biasY,
final AngularSpeed biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
fixer.setAngularSpeedBias(biasX, biasY, biasZ);
}
/**
* Gets angular speed cross-coupling errors matrix.
*
* @return cross coupling errors matrix.
*/
public Matrix getAngularSpeedCrossCouplingErrors() {
return fixer.getAngularSpeedCrossCouplingErrors();
}
/**
* Gets angular speed cross-coupling errors matrix.
*
* @param result instance where the result will be stored.
*/
public void getAngularSpeedCrossCouplingErrors(final Matrix result) {
fixer.getAngularSpeedCrossCouplingErrors(result);
}
/**
* Sets angular speed cross-coupling errors matrix.
*
* @param crossCouplingErrors cross-coupling errors matrix. Must be 3x3.
* @throws AlgebraException if matrix cannot be inverted.
* @throws IllegalArgumentException if matrix is not 3x3.
* @throws LockedException if estimator is running.
*/
public void setAngularSpeedCrossCouplingErrors(final Matrix crossCouplingErrors) throws AlgebraException,
LockedException {
if (running) {
throw new LockedException();
}
fixer.setAngularSpeedCrossCouplingErrors(crossCouplingErrors);
}
/**
* Gets angular speed x scaling factor.
*
* @return x scaling factor.
*/
public double getAngularSpeedSx() {
return fixer.getAngularSpeedSx();
}
/**
* Sets angular speed x scaling factor.
*
* @param sx x scaling factor.
* @throws LockedException if estimator is running.
* @throws AlgebraException if provided value makes angular speed
* cross-coupling matrix non-invertible.
*/
public void setAngularSpeedSx(final double sx) throws LockedException, AlgebraException {
if (running) {
throw new LockedException();
}
fixer.setAngularSpeedSx(sx);
}
/**
* Gets angular speed y scaling factor.
*
* @return y scaling factor.
*/
public double getAngularSpeedSy() {
return fixer.getAngularSpeedSy();
}
/**
* Sets angular speed y-scaling factor.
*
* @param sy y scaling factor.
* @throws LockedException if estimator is running.
* @throws AlgebraException if provided value makes angular speed
* cross-coupling matrix non-invertible.
*/
public void setAngularSpeedSy(final double sy) throws LockedException, AlgebraException {
if (running) {
throw new LockedException();
}
fixer.setAngularSpeedSy(sy);
}
/**
* Gets angular speed z scaling factor.
*
* @return z scaling factor.
*/
public double getAngularSpeedSz() {
return fixer.getAngularSpeedSz();
}
/**
* Sets angular speed z scaling factor.
*
* @param sz z scaling factor.
* @throws LockedException if estimator is running.
* @throws AlgebraException if provided value makes angular speed
* cross-coupling matrix non-invertible.
*/
public void setAngularSpeedSz(final double sz) throws LockedException, AlgebraException {
if (running) {
throw new LockedException();
}
fixer.setAngularSpeedSz(sz);
}
/**
* Gets angular speed x-y cross-coupling error.
*
* @return x-y cross coupling error.
*/
public double getAngularSpeedMxy() {
return fixer.getAngularSpeedMxy();
}
/**
* Sets angular speed x-y cross-coupling error.
*
* @param mxy x-y cross coupling error.
* @throws LockedException if estimator is running.
* @throws AlgebraException if provided value makes angular speed
* cross-coupling matrix non-invertible.
*/
public void setAngularSpeedMxy(final double mxy) throws LockedException, AlgebraException {
if (running) {
throw new LockedException();
}
fixer.setAngularSpeedMxy(mxy);
}
/**
* Gets angular speed x-z cross-coupling error.
*
* @return x-z cross coupling error.
*/
public double getAngularSpeedMxz() {
return fixer.getAngularSpeedMxz();
}
/**
* Sets angular speed x-z cross-coupling error.
*
* @param mxz x-z cross coupling error.
* @throws LockedException if estimator is running.
* @throws AlgebraException if provided value makes angular speed
* cross-coupling matrix non-invertible.
*/
public void setAngularSpeedMxz(final double mxz) throws LockedException, AlgebraException {
if (running) {
throw new LockedException();
}
fixer.setAngularSpeedMxz(mxz);
}
/**
* Gets angular speed y-x cross-coupling error.
*
* @return y-x cross coupling error.
*/
public double getAngularSpeedMyx() {
return fixer.getAngularSpeedMyx();
}
/**
* Sets angular speed y-x cross-coupling error.
*
* @param myx y-x cross coupling error.
* @throws LockedException if estimator is running.
* @throws AlgebraException if provided value makes angular speed
* cross-coupling matrix non-invertible.
*/
public void setAngularSpeedMyx(final double myx) throws LockedException, AlgebraException {
if (running) {
throw new LockedException();
}
fixer.setAngularSpeedMyx(myx);
}
/**
* Gets angular speed y-z cross-coupling error.
*
* @return y-z cross coupling error.
*/
public double getAngularSpeedMyz() {
return fixer.getAngularSpeedMyz();
}
/**
* Sets angular speed y-z cross-coupling error.
*
* @param myz y-z cross coupling error.
* @throws LockedException if estimator is running.
* @throws AlgebraException if provided value makes angular speed
* cross-coupling matrix non-invertible.
*/
public void setAngularSpeedMyz(final double myz) throws LockedException, AlgebraException {
if (running) {
throw new LockedException();
}
fixer.setAngularSpeedMyz(myz);
}
/**
* Gets angular speed z-x cross-coupling error.
*
* @return z-x cross coupling error.
*/
public double getAngularSpeedMzx() {
return fixer.getAngularSpeedMzx();
}
/**
* Sets angular speed z-x cross-coupling error.
*
* @param mzx z-x cross coupling error.
* @throws LockedException if estimator is running.
* @throws AlgebraException if provided value makes angular speed
* cross-coupling matrix non-invertible.
*/
public void setAngularSpeedMzx(final double mzx) throws LockedException, AlgebraException {
if (running) {
throw new LockedException();
}
fixer.setAngularSpeedMzx(mzx);
}
/**
* Gets angular speed z-y cross-coupling error.
*
* @return z-y cross coupling error.
*/
public double getAngularSpeedMzy() {
return fixer.getAngularSpeedMzy();
}
/**
* Sets angular speed z-y cross-coupling error.
*
* @param mzy z-y cross coupling error.
* @throws LockedException if estimator is running.
* @throws AlgebraException if provided value makes angular speed
* cross-coupling matrix non-invertible.
*/
public void setAngularSpeedMzy(final double mzy) throws LockedException, AlgebraException {
if (running) {
throw new LockedException();
}
fixer.setAngularSpeedMzy(mzy);
}
/**
* Sets angular speed scaling factors.
*
* @param sx x scaling factor.
* @param sy y scaling factor.
* @param sz z scaling factor.
* @throws LockedException if estimator is running.
* @throws AlgebraException if provided values make angular speed
* cross-coupling matrix non-invertible.
*/
public void setAngularSpeedScalingFactors(final double sx, final double sy, final double sz) throws LockedException,
AlgebraException {
if (running) {
throw new LockedException();
}
fixer.setAngularSpeedScalingFactors(sx, sy, sz);
}
/**
* Sets angular speed cross-coupling errors.
*
* @param mxy x-y cross coupling error.
* @param mxz x-z cross coupling error.
* @param myx y-x cross coupling error.
* @param myz y-z cross coupling error.
* @param mzx z-x cross coupling error.
* @param mzy z-y cross coupling error.
* @throws LockedException if estimator is running.
* @throws AlgebraException if provided values make angular speed
* cross-coupling matrix non-invertible.
*/
public void setAngularSpeedCrossCouplingErrors(
final double mxy, final double mxz,
final double myx, final double myz,
final double mzx, final double mzy) throws LockedException, AlgebraException {
if (running) {
throw new LockedException();
}
fixer.setAngularSpeedCrossCouplingErrors(mxy, mxz, myx, myz, mzx, mzy);
}
/**
* Sets angular speed scaling factors and cross-coupling errors.
*
* @param sx x scaling factor.
* @param sy y scaling factor.
* @param sz z scaling factor.
* @param mxy x-y cross coupling error.
* @param mxz x-z cross coupling error.
* @param myx y-x cross coupling error.
* @param myz y-z cross coupling error.
* @param mzx z-x cross coupling error.
* @param mzy z-y cross coupling error.
* @throws LockedException if estimator is running.
* @throws AlgebraException if provided values make angular speed
* cross-coupling matrix non-invertible.
*/
public void setAngularSpeedScalingFactorsAndCrossCouplingErrors(
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 LockedException, AlgebraException {
if (running) {
throw new LockedException();
}
fixer.setAngularSpeedScalingFactorsAndCrossCouplingErrors(sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
}
/**
* Gets angular speed g-dependant cross biases matrix.
*
* @return g-dependant cross biases matrix.
*/
public Matrix getAngularSpeedGDependantCrossBias() {
return fixer.getAngularSpeedGDependantCrossBias();
}
/**
* Gets angular speed g-dependant cross biases matrix.
*
* @param result instance where the result will be stored.
*/
public void getAngularSpeedGDependantCrossBias(final Matrix result) {
fixer.getAngularSpeedGDependantCrossBias(result);
}
/**
* Sets angular speed g-dependant cross biases matrix.
*
* @param gDependantCrossBias g-dependant cross biases matrix.
* @throws IllegalArgumentException if matrix is not 3x3.
* @throws LockedException if estimator is running.
*/
public void setAngularSpeedGDependantCrossBias(final Matrix gDependantCrossBias) throws LockedException {
if (running) {
throw new LockedException();
}
fixer.setAngularSpeedGDependantCrossBias(gDependantCrossBias);
}
/**
* Indicates whether measured kinematics must be fixed or not.
* When enabled, provided calibration data is used; otherwise it is
* ignored.
* By default, this is enabled.
*
* @return indicates whether measured kinematics must be fixed or not.
*/
public boolean isFixKinematicsEnabled() {
return fixKinematics;
}
/**
* Specifies whether measured kinematics must be fixed or not.
* When enabled, provided calibration data is used; otherwise it is
* ignored.
*
* @param fixKinematics true if measured kinematics must be fixed or not.
* @throws LockedException if estimator is currently running.
*/
public void setFixKinematicsEnabled(final boolean fixKinematics) throws LockedException {
if (running) {
throw new LockedException();
}
this.fixKinematics = fixKinematics;
}
/**
* Gets the time interval between body kinematics (IMU acceleration and gyroscope)
* samples expressed in seconds (s).
*
* @return time interval between body kinematics samples.
*/
public double getTimeInterval() {
return timeInterval;
}
/**
* Sets a time interval between body kinematics (IMU acceleration and gyroscope)
* samples expressed in seconds (s).
*
* @param timeInterval time interval between body kinematics samples.
* @throws LockedException if estimator is currently running.
*/
public void setTimeInterval(final double timeInterval) throws LockedException {
if (running) {
throw new LockedException();
}
if (timeInterval < 0.0) {
throw new IllegalArgumentException();
}
this.timeInterval = timeInterval;
}
/**
* Gets time interval between body kinematics (IMU acceleration and gyroscope)
* samples.
*
* @return time interval between body kinematics samples.
*/
public Time getTimeIntervalAsTime() {
return new Time(timeInterval, TimeUnit.SECOND);
}
/**
* Gets time interval between body kinematics (IMU acceleration and gyroscope)
* samples.
*
* @param result instance where the time interval will be stored.
*/
public void getTimeIntervalAsTime(final Time result) {
result.setValue(timeInterval);
result.setUnit(TimeUnit.SECOND);
}
/**
* Sets time interval between body kinematics (IMU acceleration and gyroscope)
* samples.
*
* @param timeInterval time interval between body kinematics samples.
* @throws LockedException if estimator is currently running.
*/
public void setTimeInterval(final Time timeInterval) throws LockedException {
setTimeInterval(convertTime(timeInterval));
}
/**
* Indicates if estimator is ready to start processing additional kinematics
* measurements.
*
* @return true if ready, false otherwise.
*/
public boolean isReady() {
return referenceFrame != null;
}
/**
* Indicates whether this estimator is running.
*
* @return true if this estimator is running, false otherwise.
*/
public boolean isRunning() {
return running;
}
/**
* Adds a sample of measured body kinematics (accelerometer and gyroscope readings)
* obtained from an IMU, fixes their values and uses fixed values to estimate
* current drift and their average values.
*
* @param kinematics measured body kinematics.
* @throws LockedException if estimator is currently running.
* @throws NotReadyException if estimator is not ready.
* @throws DriftEstimationException if estimation fails for some reason.
*/
public void addBodyKinematics(final BodyKinematics kinematics) throws LockedException, NotReadyException,
DriftEstimationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
try {
running = true;
if (numberOfProcessedSamples == 0) {
if (listener != null) {
listener.onStart(this);
}
final var c = referenceFrame.getCoordinateTransformation();
c.asRotation(refQ);
refQ.inverse(invRefQ);
frame.copyFrom(referenceFrame);
}
if (fixKinematics) {
fixer.fix(kinematics, fixedKinematics);
} else {
fixedKinematics.copyFrom(kinematics);
}
// estimate navigation variation respect to previous frame
ECEFInertialNavigator.navigateECEF(timeInterval, frame, fixedKinematics, frame);
// estimate drift values
computeCurrentPositionDrift();
computeCurrentVelocityDrift();
computeCurrentOrientationDrift();
numberOfProcessedSamples++;
if (listener != null) {
listener.onBodyKinematicsAdded(this, kinematics, fixedKinematics);
}
} catch (final AlgebraException | InertialNavigatorException | InvalidRotationMatrixException e) {
throw new DriftEstimationException(e);
} finally {
running = false;
}
}
/**
* Resets this estimator to its initial state.
*
* @throws LockedException if estimator is currently running.
*/
public void reset() throws LockedException {
if (running) {
throw new LockedException();
}
running = true;
numberOfProcessedSamples = 0;
currentPositionDriftMeters = 0.0;
currentVelocityDriftMetersPerSecond = 0.0;
currentOrientationDriftRadians = 0.0;
if (listener != null) {
listener.onReset(this);
}
running = false;
}
/**
* Gets the number of samples that have been processed so far.
*
* @return number of samples that have been processed so far.
*/
public int getNumberOfProcessedSamples() {
return numberOfProcessedSamples;
}
/**
* Gets elapsed time since the first processed measurement expressed in seconds.
*
* @return elapsed time.
*/
public double getElapsedTimeSeconds() {
return numberOfProcessedSamples * timeInterval;
}
/**
* Gets elapsed time since the first processed measurement.
*
* @return elapsed time.
*/
public Time getElapsedTime() {
return numberOfProcessedSamples > 0 ? new Time(getElapsedTimeSeconds(), TimeUnit.SECOND) : null;
}
/**
* Gets elapsed time since the first processed measurement.
*
* @param result instance where the result will be stored.
* @return true if elapsed time is available and the result is updated,
* false otherwise.
*/
public boolean getElapsedTime(final Time result) {
if (numberOfProcessedSamples > 0) {
result.setValue(getElapsedTimeSeconds());
result.setUnit(TimeUnit.SECOND);
return true;
} else {
return false;
}
}
/**
* Gets current position drift for last processed body kinematics
* measurement expressed in meters (m) respect ECEF coordinates.
*
* @return current position drift or null.
*/
public ECEFPosition getCurrentPositionDrift() {
return numberOfProcessedSamples > 0 ? new ECEFPosition(currentPositionDrift) : null;
}
/**
* Gets current position drift for last processed body kinematics
* measurement expressed in meters (m) respect ECEF coordinates.
*
* @param result instance where the result will be stored.
* @return true if the current position drift is available and the result is updated,
* false otherwise.
*/
public boolean getCurrentPositionDrift(final ECEFPosition result) {
if (numberOfProcessedSamples > 0) {
result.copyFrom(currentPositionDrift);
return true;
} else {
return false;
}
}
/**
* Gets current velocity drift for the last processed body kinematics
* measurement expressed in meters per second (m/s) respect ECEF coordinates.
*
* @return current velocity drift or null.
*/
public ECEFVelocity getCurrentVelocityDrift() {
return numberOfProcessedSamples > 0 ? new ECEFVelocity(currentVelocityDrift) : null;
}
/**
* Gets current velocity drift for the last processed body kinematics
* measurement expressed in meters per second (m/s) respect ECEF coordinates.
*
* @param result instance where the result will be stored.
* @return true if the current velocity drift is available and the result is updated,
* false otherwise.
*/
public boolean getCurrentVelocityDrift(final ECEFVelocity result) {
if (numberOfProcessedSamples > 0) {
result.copyFrom(currentVelocityDrift);
return true;
} else {
return false;
}
}
/**
* Gets current orientation drift as a 3D rotation for the last processed body
* kinematics measurement respect ECEF coordinates.
*
* @return current orientation drift or null.
*/
public Rotation3D getCurrentOrientationDrift() {
return numberOfProcessedSamples > 0 ? new Quaternion(q) : null;
}
/**
* Gets current orientation drift as a 3D rotation for the last processed body
* kinematics measurement respect ECEF coordinates.
*
* @param result instance where the result will be stored.
* @return true if the current orientation drift is available and the result is
* updated, false otherwise.
*/
public boolean getCurrentOrientationDrift(final Rotation3D result) {
if (numberOfProcessedSamples > 0) {
result.fromRotation(q);
return true;
} else {
return false;
}
}
/**
* Gets current amount of position drift for last processed body kinematics
* measurement expressed in meters (m).
*
* @return the current amount of position drift or null.
*/
public Double getCurrentPositionDriftNormMeters() {
return numberOfProcessedSamples > 0 ? currentPositionDriftMeters : null;
}
/**
* Gets current amount of position drift for last processed body kinematics
* measurement.
*
* @return the current amount of position drift or null.
*/
public Distance getCurrentPositionDriftNorm() {
final var positionDrift = getCurrentPositionDriftNormMeters();
return positionDrift != null ? new Distance(positionDrift, DistanceUnit.METER) : null;
}
/**
* Gets current amount of position drift for last processed body kinematics
* measurement.
*
* @param result instance where the result will be stored.
* @return true if the current position drift is available and the result is updated,
* false otherwise.
*/
public boolean getCurrentPositionDriftNorm(final Distance result) {
final var positionDrift = getCurrentPositionDriftNormMeters();
if (positionDrift != null) {
result.setValue(positionDrift);
result.setUnit(DistanceUnit.METER);
return true;
} else {
return false;
}
}
/**
* Gets current amount of velocity drift for the last processed body kinematics
* measurement expressed in meters per second (m/s).
*
* @return current amount of velocity drift or null.
*/
public Double getCurrentVelocityDriftNormMetersPerSecond() {
return numberOfProcessedSamples > 0 ? currentVelocityDriftMetersPerSecond : null;
}
/**
* Gets current amount of velocity drift for last processed body kinematics
* measurement.
*
* @return current amount of velocity drift or null.
*/
public Speed getCurrentVelocityDriftNorm() {
final var velocityDrift = getCurrentVelocityDriftNormMetersPerSecond();
return velocityDrift != null ? new Speed(velocityDrift, SpeedUnit.METERS_PER_SECOND) : null;
}
/**
* Gets current amount of velocity drift for last processed body kinematics
* measurement.
*
* @param result instance where the result will be stored.
* @return true if the current velocity drift is available and the result is updated,
* false otherwise.
*/
public boolean getCurrentVelocityDriftNorm(final Speed result) {
final var velocityDrift = getCurrentVelocityDriftNormMetersPerSecond();
if (velocityDrift != null) {
result.setValue(velocityDrift);
result.setUnit(SpeedUnit.METERS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets current amount of orientation drift for last processed body kinematics
* measurement expressed in radians (rad).
*
* @return current amount of orientation drift or null.
*/
public Double getCurrentOrientationDriftRadians() {
return numberOfProcessedSamples > 0 ? currentOrientationDriftRadians : null;
}
/**
* Gets current amount of orientation drift for last processed body kinematics
* measurement.
*
* @return current amount of orientation drift or null.
*/
public Angle getCurrentOrientationDriftAngle() {
final var orientationDrift = getCurrentOrientationDriftRadians();
return orientationDrift != null ? new Angle(orientationDrift, AngleUnit.RADIANS) : null;
}
/**
* Gets current amount of orientation drift for last processed body kinematics
* measurement.
*
* @param result instance where the result will be stored.
* @return true if the current orientation drift is available and the result is
* updated, false otherwise.
*/
public boolean getCurrentOrientationDriftAngle(final Angle result) {
final var orientationDrift = getCurrentOrientationDriftRadians();
if (orientationDrift != null) {
result.setValue(orientationDrift);
result.setUnit(AngleUnit.RADIANS);
return true;
} else {
return false;
}
}
/**
* Gets the current amount of position drift per time unit expressed in meters
* per second (m/s).
*
* @return current amount of position drift per time unit or null.
*/
public Double getCurrentPositionDriftPerTimeUnit() {
final var positionDrift = getCurrentPositionDriftNormMeters();
final var elapsedTime = getElapsedTimeSeconds();
return positionDrift != null ? positionDrift / elapsedTime : null;
}
/**
* Gets the current amount of position drift per time unit.
*
* @return current amount of position drift per time unit or null.
*/
public Speed getCurrentPositionDriftPerTimeUnitAsSpeed() {
final var positionDriftPerTimeUnit = getCurrentPositionDriftPerTimeUnit();
return positionDriftPerTimeUnit != null ? new Speed(positionDriftPerTimeUnit, SpeedUnit.METERS_PER_SECOND)
: null;
}
/**
* Gets the current amount of position drift per time unit.
*
* @param result instance where the result will be stored.
* @return true if the current amount of position drift per time unit is
* available and the result is updated, false otherwise.
*/
public boolean getCurrentPositionDriftPerTimeUnitAsSpeed(final Speed result) {
final var positionDriftPerTimeUnit = getCurrentPositionDriftPerTimeUnit();
if (positionDriftPerTimeUnit != null) {
result.setValue(positionDriftPerTimeUnit);
result.setUnit(SpeedUnit.METERS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets the current amount of velocity drift per time unit expressed in meters
* per squared second (m/s^2).
*
* @return the current amount of velocity drift per time unit or null.
*/
public Double getCurrentVelocityDriftPerTimeUnit() {
final var velocityDrift = getCurrentVelocityDriftNormMetersPerSecond();
final var elapsedTime = getElapsedTimeSeconds();
return velocityDrift != null ? velocityDrift / elapsedTime : null;
}
/**
* Gets the current amount of velocity drift per time unit.
*
* @return the current amount of velocity drift per time unit or null.
*/
public Acceleration getCurrentVelocityDriftPerTimeUnitAsAcceleration() {
final var velocityDriftPerTimeUnit = getCurrentVelocityDriftPerTimeUnit();
return velocityDriftPerTimeUnit != null
? new Acceleration(velocityDriftPerTimeUnit, AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
}
/**
* Gets the current amount of velocity drift per time unit.
*
* @param result instance where the result will be stored.
* @return true if the current amount of velocity drift per time unit is available
* and the result is updated, false otherwise.
*/
public boolean getCurrentVelocityDriftPerTimeUnitAsAcceleration(final Acceleration result) {
final var velocityDriftPerTimeUnit = getCurrentVelocityDriftPerTimeUnit();
if (velocityDriftPerTimeUnit != null) {
result.setValue(velocityDriftPerTimeUnit);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets the current amount of orientation drift per time unit expressed in radians
* per second (rad/s).
*
* @return amount of orientation drift per time unit or null.
*/
public Double getCurrentOrientationDriftPerTimeUnit() {
final var orientationDrift = getCurrentOrientationDriftRadians();
final var elapsedTime = getElapsedTimeSeconds();
return orientationDrift != null ? orientationDrift / elapsedTime : null;
}
/**
* Gets the current amount of orientation drift per time unit.
*
* @return amount of orientation drift per time unit or null.
*/
public AngularSpeed getCurrentOrientationDriftPerTimeUnitAsAngularSpeed() {
final var orientationDriftPerTimeUnit = getCurrentOrientationDriftPerTimeUnit();
return orientationDriftPerTimeUnit != null ?
new AngularSpeed(orientationDriftPerTimeUnit, AngularSpeedUnit.RADIANS_PER_SECOND) : null;
}
/**
* Gets the current amount of orientation drift per time unit.
*
* @param result instance where the result will be stored.
* @return true if the current amount of orientation drift is available and the result
* is updated, false otherwise.
*/
public boolean getCurrentOrientationDriftPerTimeUnitAsAngularSpeed(final AngularSpeed result) {
final var orientationDriftPerTimeUnit = getCurrentOrientationDriftPerTimeUnit();
if (orientationDriftPerTimeUnit != null) {
result.setValue(orientationDriftPerTimeUnit);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Computes current position drift.
*/
protected void computeCurrentPositionDrift() {
final var initX = referenceFrame.getX();
final var initY = referenceFrame.getY();
final var initZ = referenceFrame.getZ();
final var currentX = frame.getX();
final var currentY = frame.getY();
final var currentZ = frame.getZ();
final var diffX = currentX - initX;
final var diffY = currentY - initY;
final var diffZ = currentZ - initZ;
currentPositionDrift.setCoordinates(diffX, diffY, diffZ);
currentPositionDriftMeters = currentPositionDrift.getNorm();
}
/**
* Computes current velocity drift.
*/
protected void computeCurrentVelocityDrift() {
final var initVx = referenceFrame.getVx();
final var initVy = referenceFrame.getVy();
final var initVz = referenceFrame.getVz();
final var currentVx = frame.getVx();
final var currentVy = frame.getVy();
final var currentVz = frame.getVz();
final var diffVx = currentVx - initVx;
final var diffVy = currentVy - initVy;
final var diffVz = currentVz - initVz;
currentVelocityDrift.setCoordinates(diffVx, diffVy, diffVz);
currentVelocityDriftMetersPerSecond = currentVelocityDrift.getNorm();
}
/**
* Computes current orientation drift.
*
* @throws AlgebraException if there are numerical instabilities.
* @throws InvalidRotationMatrixException if rotation cannot be accurately
* estimated.
*/
protected void computeCurrentOrientationDrift() throws AlgebraException, InvalidRotationMatrixException {
if (currentC == null) {
currentC = new Matrix(Rotation3D.INHOM_COORDS, Rotation3D.INHOM_COORDS);
}
frame.getCoordinateTransformationMatrix(currentC);
q.fromMatrix(currentC);
q.combine(invRefQ);
currentOrientationDriftRadians = q.getRotationAngle();
}
/**
* Converts the provided time instance to seconds.
*
* @param time instance to be converted.
* @return conversion in seconds.
*/
private static double convertTime(final Time time) {
return TimeConverter.convert(time.getValue().doubleValue(), time.getUnit(), TimeUnit.SECOND);
}
}