GNSSLeastSquaresPositionAndVelocityEstimator.java
/*
* Copyright (C) 2019 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.gnss;
import com.irurueta.algebra.AlgebraException;
import com.irurueta.algebra.Matrix;
import com.irurueta.algebra.Utils;
import com.irurueta.algebra.WrongSizeException;
import com.irurueta.navigation.LockedException;
import com.irurueta.navigation.NotReadyException;
import com.irurueta.navigation.frames.ECEFPosition;
import com.irurueta.navigation.frames.ECEFVelocity;
import com.irurueta.navigation.frames.NEDPosition;
import com.irurueta.navigation.frames.NEDVelocity;
import com.irurueta.navigation.frames.converters.ECEFtoNEDPositionVelocityConverter;
import com.irurueta.navigation.frames.converters.NEDtoECEFPositionVelocityConverter;
import com.irurueta.navigation.geodesic.Constants;
import java.util.Collection;
/**
* Calculates position, velocity, clock offset and clock drift using
* unweighted iterated least squares.
* Separate calculations are implemented for position and clock offset and
* for velocity and clock drift.
* This implementation is based on the equations defined in "Principles of GNSS, Inertial, and Multi-sensor
* Integrated Navigation Systems, Second Edition" and on the companion software available at:
* <a href="https://github.com/ymjdz/MATLAB-Codes/blob/master/GNSS_LS_position_velocity.m">
* https://github.com/ymjdz/MATLAB-Codes/blob/master/GNSS_LS_position_velocity.m
* </a>
*/
public class GNSSLeastSquaresPositionAndVelocityEstimator {
/**
* Minimum number of measurements required to obtain a solution.
*/
public static final int MIN_MEASUREMENTS = 4;
/**
* Default threshold to determine when convergence has been reached.
*/
public static final double CONVERGENCE_THRESHOLD = 1e-4;
/**
* Speed of light in the vacuum expressed in meters per second (m/s).
*/
public static final double SPEED_OF_LIGHT = Constants.SPEED_OF_LIGHT;
/**
* Earth rotation rate expressed in radians per second (rad/s).
*/
public static final double EARTH_ROTATION_RATE = Constants.EARTH_ROTATION_RATE;
/**
* Number of components of predicted state.
*/
private static final int STATE_COMPONENTS = ECEFPosition.COMPONENTS + 1;
/**
* Number of elements of position, velocity, etc.
*/
private static final int ELEMS = ECEFPosition.COMPONENTS;
/**
* Number of elements of position minus one.
*/
private static final int ELEMS_MINUS_ONE = ELEMS - 1;
/**
* GNSS measurements of a collection of satellites.
*/
private Collection<GNSSMeasurement> measurements;
/**
* Previously predicted ECEF user position and velocity.
*/
private ECEFPositionAndVelocity priorPositionAndVelocity;
/**
* Listener to notify events raised by this instance.
*/
private GNSSLeastSquaresPositionAndVelocityEstimatorListener listener;
/**
* Threshold to determine when convergence has been reached.
*/
private double convergenceThreshold = CONVERGENCE_THRESHOLD;
/**
* Indicates whether estimation is currently running.
*/
private boolean running;
/**
* Internal matrix to be reused containing frame rotation during signal transit
* time.
*/
private final Matrix cei;
/**
* Predicted state to be reused.
*/
private final Matrix xPred;
/**
* Temporal matrix to be reused.
*/
private final Matrix tmp1;
/**
* Temporal matrix to be reused.
*/
private final Matrix tmp2;
/**
* Estimated state to be reused.
*/
private final Matrix xEst;
/**
* Contains square representation of measurement or geometry matrix.
*/
private final Matrix hSqr;
/**
* Inverse of the square representation of measurement or geometry matrix.
*/
private final Matrix invHSqr;
/**
* Temporal matrix to be reused.
*/
private final Matrix tmp3;
/**
* Skew symmetric matrix of Earth rotation rate.
*/
private final Matrix omegaIe;
/**
* Temporal matrix to be reused.
*/
private final Matrix tmp4;
/**
* Temporal matrix to be reused.
*/
private final Matrix tmp5;
/**
* Temporal matrix to be reused.
*/
private final Matrix tmp6;
/**
* Temporal matrix to be reused.
*/
private final Matrix tmp7;
/**
* Temporal matrix to be reused.
*/
private final Matrix tmp8;
/**
* Temporal matrix to be reused.
*/
private final Matrix tmp9;
/**
* Temporal matrix to be reused.
*/
private final Matrix tmp10;
/**
* Measurement position to be reused.
*/
private final Matrix measurementPosition;
/**
* Measurement velocity to be reused.
*/
private final Matrix measurementVelocity;
/**
* Predicted velocity to be reused.
*/
private final Matrix predVelocity;
/**
* Result position to be reused.
*/
private final Matrix resultPosition;
/**
* Constructor.
*/
public GNSSLeastSquaresPositionAndVelocityEstimator() {
Matrix cxPred = null;
Matrix ctmp1 = null;
Matrix ctmp2 = null;
Matrix cxEst = null;
Matrix chSqr = null;
Matrix cinvHSqr = null;
Matrix ctmp3 = null;
Matrix comegaIe = null;
Matrix ccei = null;
Matrix ctmp4 = null;
Matrix ctmp5 = null;
Matrix ctmp6 = null;
Matrix ctmp7 = null;
Matrix ctmp8 = null;
Matrix ctmp9 = null;
Matrix ctmp10 = null;
Matrix cmeasurementPosition = null;
Matrix cmeasurementVelocity = null;
Matrix cpredVelocity = null;
Matrix cresultPosition = null;
try {
ccei = Matrix.identity(ELEMS, ELEMS);
cxPred = new Matrix(STATE_COMPONENTS, 1);
ctmp1 = new Matrix(ELEMS, 1);
ctmp2 = new Matrix(ELEMS, 1);
cxEst = new Matrix(STATE_COMPONENTS, 1);
chSqr = new Matrix(STATE_COMPONENTS, STATE_COMPONENTS);
cinvHSqr = new Matrix(STATE_COMPONENTS, STATE_COMPONENTS);
ctmp3 = new Matrix(STATE_COMPONENTS, 1);
comegaIe = Utils.skewMatrix(new double[]{0.0, 0.0, EARTH_ROTATION_RATE});
ctmp4 = new Matrix(STATE_COMPONENTS, 1);
ctmp5 = new Matrix(STATE_COMPONENTS, 1);
ctmp6 = new Matrix(STATE_COMPONENTS, 1);
ctmp7 = new Matrix(STATE_COMPONENTS, 1);
ctmp8 = new Matrix(STATE_COMPONENTS, 1);
ctmp9 = new Matrix(STATE_COMPONENTS, 1);
ctmp10 = new Matrix(STATE_COMPONENTS, 1);
cmeasurementPosition = new Matrix(ELEMS, 1);
cmeasurementVelocity = new Matrix(ELEMS, 1);
cpredVelocity = new Matrix(ELEMS, 1);
cresultPosition = new Matrix(ELEMS, 1);
} catch (WrongSizeException ignore) {
// never happens
}
this.cei = ccei;
this.xPred = cxPred;
this.tmp1 = ctmp1;
this.tmp2 = ctmp2;
this.xEst = cxEst;
this.hSqr = chSqr;
this.invHSqr = cinvHSqr;
this.tmp3 = ctmp3;
this.omegaIe = comegaIe;
this.tmp4 = ctmp4;
this.tmp5 = ctmp5;
this.tmp6 = ctmp6;
this.tmp7 = ctmp7;
this.tmp8 = ctmp8;
this.tmp9 = ctmp9;
this.tmp10 = ctmp10;
this.measurementPosition = cmeasurementPosition;
this.measurementVelocity = cmeasurementVelocity;
this.predVelocity = cpredVelocity;
this.resultPosition = cresultPosition;
}
/**
* Constructor.
*
* @param measurements GNSS measurements of a collection of satellites.
* @throws IllegalArgumentException if less than 4 measurements are provided.
*/
public GNSSLeastSquaresPositionAndVelocityEstimator(final Collection<GNSSMeasurement> measurements) {
this();
try {
setMeasurements(measurements);
} catch (final LockedException ignore) {
// never happens
}
}
/**
* Constructor.
*
* @param measurements GNSS measurements of a collection of satellites.
* @param priorPositionAndVelocity previously predicted ECEF user position and
* velocity.
* @throws IllegalArgumentException if less than 4 measurements are provided.
*/
public GNSSLeastSquaresPositionAndVelocityEstimator(
final Collection<GNSSMeasurement> measurements,
final ECEFPositionAndVelocity priorPositionAndVelocity) {
this();
try {
setMeasurements(measurements);
setPriorPositionAndVelocity(priorPositionAndVelocity);
} catch (final LockedException ignore) {
// never happens
}
}
/**
* Constructor.
*
* @param measurements GNSS measurement of a collection of satellites.
* @param priorEstimation previously predicted GNSS estimation.
* @throws IllegalArgumentException if less than 4 measurements are provided.
*/
public GNSSLeastSquaresPositionAndVelocityEstimator(
final Collection<GNSSMeasurement> measurements,
final GNSSEstimation priorEstimation) {
this();
try {
setMeasurements(measurements);
setPriorPositionAndVelocityFromEstimation(priorEstimation);
} catch (final LockedException ignore) {
// never happens
}
}
/**
* Constructor.
*
* @param listener listener notifying events raised by this instance.
*/
public GNSSLeastSquaresPositionAndVelocityEstimator(
final GNSSLeastSquaresPositionAndVelocityEstimatorListener listener) {
this();
try {
setListener(listener);
} catch (final LockedException ignore) {
// never happens
}
}
/**
* Constructor.
*
* @param measurements GNSS measurements of a collection of satellites.
* @param listener listener notifying events raised by this instance.
* @throws IllegalArgumentException if less than 4 measurements are provided.
*/
public GNSSLeastSquaresPositionAndVelocityEstimator(
final Collection<GNSSMeasurement> measurements,
final GNSSLeastSquaresPositionAndVelocityEstimatorListener listener) {
this(measurements);
try {
setListener(listener);
} catch (final LockedException ignore) {
// never happens
}
}
/**
* Constructor.
*
* @param measurements GNSS measurements of a collection of satellites.
* @param priorPositionAndVelocity previously predicted ECEF user position and
* velocity.
* @param listener listener notifying events raised by this
* instance.
* @throws IllegalArgumentException if less than 4 measurements are provided.
*/
public GNSSLeastSquaresPositionAndVelocityEstimator(
final Collection<GNSSMeasurement> measurements,
final ECEFPositionAndVelocity priorPositionAndVelocity,
final GNSSLeastSquaresPositionAndVelocityEstimatorListener listener) {
this(measurements, priorPositionAndVelocity);
try {
setListener(listener);
} catch (final LockedException ignore) {
// never happens
}
}
/**
* Constructor.
*
* @param measurements GNSS measurement of a collection of satellites.
* @param priorEstimation previously predicted GNSS estimation.
* @param listener listener notifying events raised by this instance.
* @throws IllegalArgumentException if less than 4 measurements are provided.
*/
public GNSSLeastSquaresPositionAndVelocityEstimator(
final Collection<GNSSMeasurement> measurements,
final GNSSEstimation priorEstimation,
final GNSSLeastSquaresPositionAndVelocityEstimatorListener listener) {
this(measurements, priorEstimation);
try {
setListener(listener);
} catch (final LockedException ignore) {
// never happens
}
}
/**
* Gets GNSS measurements of a collection of satellites.
*
* @return GNSS measurements of a collection of satellites.
*/
public Collection<GNSSMeasurement> getMeasurements() {
return measurements;
}
/**
* Sets GNSS measurements of a collection of satellites.
*
* @param measurements GNSS measurements of a collection of satellites.
* @throws IllegalArgumentException if less than 4 measurements are provided.
* @throws LockedException if this estimator is already running.
*/
public void setMeasurements(final Collection<GNSSMeasurement> measurements) throws LockedException {
if (running) {
throw new LockedException();
}
if (!isValidMeasurements(measurements)) {
throw new IllegalArgumentException();
}
this.measurements = measurements;
}
/**
* Gets previously predicted ECEF user position and velocity.
*
* @return previously predicted ECEF user position and velocity.
*/
public ECEFPositionAndVelocity getPriorPositionAndVelocity() {
return priorPositionAndVelocity;
}
/**
* Sets previously predicted ECEF user position and velocity.
*
* @param priorPositionAndVelocity previously predicted ECEF user position and
* velocity.
* @throws LockedException if this estimator is already running.
*/
public void setPriorPositionAndVelocity(
final ECEFPositionAndVelocity priorPositionAndVelocity) throws LockedException {
if (running) {
throw new LockedException();
}
this.priorPositionAndVelocity = priorPositionAndVelocity;
}
/**
* Sets previously predicted ECEF user position and velocity from a previous
* predicted result.
*
* @param priorEstimation previously predicted GNSS estimation.
* @throws LockedException if this estimator is already running.
*/
public void setPriorPositionAndVelocityFromEstimation(
final GNSSEstimation priorEstimation) throws LockedException {
if (running) {
throw new LockedException();
}
priorPositionAndVelocity = priorEstimation != null ? priorEstimation.getPositionAndVelocity() : null;
}
/**
* Gets listener to notify events raised by this instance.
*
* @return listener to notify events raised by this instance.
*/
public GNSSLeastSquaresPositionAndVelocityEstimatorListener getListener() {
return listener;
}
/**
* Sets listener to notify events raised by this instance.
*
* @param listener listener to notify events raised by this instance.
* @throws LockedException if this estimator is already running.
*/
public void setListener(
final GNSSLeastSquaresPositionAndVelocityEstimatorListener listener) throws LockedException {
if (running) {
throw new LockedException();
}
this.listener = listener;
}
/**
* Gets threshold to determine when convergence has been reached.
*
* @return threshold to determine when convergence has been reached.
*/
public double getConvergenceThreshold() {
return convergenceThreshold;
}
/**
* Sets threshold to determine when convergence has been reached.
*
* @param convergenceThreshold threshold to determine when convergence has
* been reached.
* @throws LockedException if this estimator is already running.
* @throws IllegalArgumentException if provided threshold is zero or negative.
*/
public void setConvergenceThreshold(final double convergenceThreshold) throws LockedException,
IllegalArgumentException {
if (running) {
throw new LockedException();
}
if (convergenceThreshold <= 0.0) {
throw new IllegalArgumentException();
}
this.convergenceThreshold = convergenceThreshold;
}
/**
* Indicates whether this estimator is ready to start the estimation.
*
* @return true if estimator is ready, false otherwise.
*/
public boolean isReady() {
return isValidMeasurements(measurements);
}
/**
* Indicates whether this estimator is currently running or not.
*
* @return true if estimator is running, false otherwise.
*/
public boolean isRunning() {
return running;
}
/**
* Indicates whether provided measurements are valid or not.
*
* @param gnssMeasurements measurements to be checked.
* @return true if at least 4 measurements are provided, false otherwise.
*/
public static boolean isValidMeasurements(final Collection<GNSSMeasurement> gnssMeasurements) {
return gnssMeasurements != null && gnssMeasurements.size() >= MIN_MEASUREMENTS;
}
/**
* Estimates new ECEF user position and velocity as well as clock
* offset and drift.
*
* @param result instance where result data will be stored.
* @throws NotReadyException if estimator is not ready to start estimation.
* @throws LockedException if estimator is already running.
* @throws GNSSException if estimation fails due to numerical instabilities.
*/
@SuppressWarnings("DuplicatedCode")
public void estimate(final GNSSEstimation result) throws NotReadyException, LockedException, GNSSException {
if (!isReady()) {
throw new NotReadyException();
}
if (running) {
throw new LockedException();
}
try {
running = true;
if (listener != null) {
listener.onEstimateStart(this);
}
// if no prior position and velocity is available, assume that
// we are at latitude,longitude equal to the average of satellite
// measurements, at Earth's surface (height = 0) and with zero velocity.
initializePriorPositionAndVelocityIfNeeded();
// POSITION AND CLOCK OFFSET
// Setup predicted state
final var priorX = priorPositionAndVelocity.getX();
final var priorY = priorPositionAndVelocity.getY();
final var priorZ = priorPositionAndVelocity.getZ();
xPred.setElementAtIndex(0, priorX);
xPred.setElementAtIndex(1, priorY);
xPred.setElementAtIndex(2, priorZ);
xPred.setElementAtIndex(3, 0.0);
final var numMeasurements = measurements.size();
final var predMeas = new Matrix(numMeasurements, 1);
final var h = new Matrix(numMeasurements, STATE_COMPONENTS);
for (var i = 0; i < numMeasurements; i++) {
h.setElementAt(i, 3, 1.0);
}
final var hTrans = new Matrix(STATE_COMPONENTS, numMeasurements);
final var hTmp1 = new Matrix(STATE_COMPONENTS, numMeasurements);
final var deltaPseudoRange = new Matrix(numMeasurements, 1);
// Repeat until convergence
var testConvergence = 1.0;
while (testConvergence > convergenceThreshold) {
// Loop measurements
var j = 0;
for (final var measurement : measurements) {
// Predict approx range
final var measX = measurement.getX();
final var measY = measurement.getY();
final var measZ = measurement.getZ();
var deltaRx = measX - priorX;
var deltaRy = measY - priorY;
var deltaRz = measZ - priorZ;
final var approxRange = norm(deltaRx, deltaRy, deltaRz);
// Calculate frame rotation during signal transit time using (8.36)
final var ceiValue = EARTH_ROTATION_RATE * approxRange / SPEED_OF_LIGHT;
cei.setElementAt(0, 1, ceiValue);
cei.setElementAt(1, 0, -ceiValue);
// Predict pseudo-range using (9.143)
tmp1.setElementAtIndex(0, measX);
tmp1.setElementAtIndex(1, measY);
tmp1.setElementAtIndex(2, measZ);
cei.multiply(tmp1, tmp2);
deltaRx = tmp2.getElementAtIndex(0) - xPred.getElementAtIndex(0);
deltaRy = tmp2.getElementAtIndex(1) - xPred.getElementAtIndex(1);
deltaRz = tmp2.getElementAtIndex(2) - xPred.getElementAtIndex(2);
final var range = norm(deltaRx, deltaRy, deltaRz);
final var predictedPseudoRange = range + xPred.getElementAtIndex(3);
predMeas.setElementAtIndex(j, predictedPseudoRange);
deltaPseudoRange.setElementAtIndex(j, measurement.getPseudoRange() - predictedPseudoRange);
// Predict line of sight and deploy in measurement matrix, (9.144)
h.setElementAt(j, 0, -deltaRx / range);
h.setElementAt(j, 1, -deltaRy / range);
h.setElementAt(j, 2, -deltaRz / range);
j++;
}
// Unweighted least-squares solution, (9.35)/(9.141)
h.transpose(hTrans);
hTrans.multiply(h, hSqr);
Utils.inverse(hSqr, invHSqr);
invHSqr.multiply(hTrans, hTmp1);
hTmp1.multiply(deltaPseudoRange, tmp3);
xPred.add(tmp3, xEst);
// Test convergence
testConvergence = predictionError();
// Set predictions to estimates for next iteration
xPred.copyFrom(xEst);
}
// Set outputs to estimates
final var resultX = xEst.getElementAtIndex(0);
final var resultY = xEst.getElementAtIndex(1);
final var resultZ = xEst.getElementAtIndex(2);
result.setPositionCoordinates(resultX, resultY, resultZ);
final var resultClockOffset = xEst.getElementAtIndex(3);
result.setClockOffset(resultClockOffset);
// VELOCITY AND CLOCK DRIFT
// Setup predicted state
final var priorVx = priorPositionAndVelocity.getVx();
final var priorVy = priorPositionAndVelocity.getVy();
final var priorVz = priorPositionAndVelocity.getVz();
xPred.setElementAtIndex(0, priorVx);
xPred.setElementAtIndex(1, priorVy);
xPred.setElementAtIndex(2, priorVz);
xPred.setElementAtIndex(3, 0.0);
resultPosition.setElementAtIndex(0, resultX);
resultPosition.setElementAtIndex(1, resultY);
resultPosition.setElementAtIndex(2, resultZ);
final var deltaPseudoRangeRate = new Matrix(numMeasurements, 1);
// Repeat until convergence
testConvergence = 1.0;
while (testConvergence > convergenceThreshold) {
// Loop measurements
var j = 0;
for (final var measurement : measurements) {
// Predict approx range
final var measX = measurement.getX();
final var measY = measurement.getY();
final var measZ = measurement.getZ();
var deltaRx = measX - resultX;
var deltaRy = measY - resultY;
var deltaRz = measZ - resultZ;
final var approxRange = norm(deltaRx, deltaRy, deltaRz);
// Calculate frame rotation during signal transit time using (8.36)
final var ceiValue = EARTH_ROTATION_RATE * approxRange / SPEED_OF_LIGHT;
cei.setElementAt(0, 1, ceiValue);
cei.setElementAt(1, 0, -ceiValue);
// Calculate range using (8.35)
tmp1.setElementAtIndex(0, measX);
tmp1.setElementAtIndex(1, measY);
tmp1.setElementAtIndex(2, measZ);
cei.multiply(tmp1, tmp2);
deltaRx = tmp2.getElementAtIndex(0) - resultX;
deltaRy = tmp2.getElementAtIndex(1) - resultY;
deltaRz = tmp2.getElementAtIndex(2) - resultZ;
final var range = norm(deltaRx, deltaRy, deltaRz);
// Calculate line of sight using (8.41)
final var uaseX = deltaRx / range;
final var uaseY = deltaRy / range;
final var uaseZ = deltaRz / range;
// Predict pseudo-range rate using (9.143)
measurementPosition.setElementAtIndex(0, measX);
measurementPosition.setElementAtIndex(1, measY);
measurementPosition.setElementAtIndex(2, measZ);
final var measVx = measurement.getVx();
final var measVy = measurement.getVy();
final var measVz = measurement.getVz();
measurementVelocity.setElementAtIndex(0, measVx);
measurementVelocity.setElementAtIndex(1, measVy);
measurementVelocity.setElementAtIndex(2, measVz);
omegaIe.multiply(measurementPosition, tmp4);
measurementVelocity.add(tmp4, tmp5);
cei.multiply(tmp5, tmp6);
omegaIe.multiply(resultPosition, tmp7);
xPred.getSubmatrix(0, 0, ELEMS_MINUS_ONE, 0, predVelocity);
predVelocity.add(tmp7, tmp8);
tmp6.subtract(tmp8, tmp9);
final var rangeRate = uaseX * tmp9.getElementAtIndex(0) + uaseY * tmp9.getElementAtIndex(1)
+ uaseZ * tmp9.getElementAtIndex(2);
final var predictedPseudoRangeRate = rangeRate + xPred.getElementAtIndex(3);
predMeas.setElementAtIndex(j, predictedPseudoRangeRate);
deltaPseudoRangeRate.setElementAtIndex(j,
measurement.getPseudoRate() - predictedPseudoRangeRate);
// Predict line of sight and deploy in measurement matrix, (9.144)
h.setElementAt(j, 0, -uaseX);
h.setElementAt(j, 1, -uaseY);
h.setElementAt(j, 2, -uaseZ);
j++;
}
// Unweighted least-squares solution, (9.35)/(9.141)
h.transpose(hTrans);
hTrans.multiply(h, hSqr);
Utils.inverse(hSqr, invHSqr);
invHSqr.multiply(hTrans, hTmp1);
hTmp1.multiply(deltaPseudoRangeRate, tmp10);
xPred.add(tmp10, xEst);
// Test convergence
testConvergence = predictionError();
// Set predictions to estimates for next iteration
xPred.copyFrom(xEst);
}
// Set outputs to estimates
final var resultVx = xEst.getElementAtIndex(0);
final var resultVy = xEst.getElementAtIndex(1);
final var resultVz = xEst.getElementAtIndex(2);
result.setVelocityCoordinates(resultVx, resultVy, resultVz);
final var resultClockDrift = xEst.getElementAtIndex(3);
result.setClockDrift(resultClockDrift);
} catch (final AlgebraException e) {
throw new GNSSException(e);
} finally {
if (listener != null) {
listener.onEstimateEnd(this);
}
running = false;
}
}
/**
* Estimates new ECEF user position and velocity as well as clock
* offset and drift.
*
* @return new ECEF user position and velocity, and clock offset and drift.
* @throws NotReadyException if estimator is not ready to start estimation.
* @throws LockedException if estimator is already running.
* @throws GNSSException if estimation fails due to numerical instabilities.
*/
public GNSSEstimation estimate() throws NotReadyException, LockedException, GNSSException {
final var result = new GNSSEstimation();
estimate(result);
return result;
}
/**
* Initializes prior position and velocity if not set, assuming that
* user is located at the average latitude, longitude of all provided
* satellite measurements, at Earth's surface (height = 0) and with zero velocity.
*/
private void initializePriorPositionAndVelocityIfNeeded() {
if (priorPositionAndVelocity != null) {
return;
}
var numMeasurements = measurements.size();
final var nedPosition = new NEDPosition();
final var nedVelocity = new NEDVelocity();
final var ecefPosition = new ECEFPosition();
final var ecefVelocity = new ECEFVelocity();
var userLatitude = 0.0;
var userLongitude = 0.0;
for (final var measurement : measurements) {
measurement.getEcefPosition(ecefPosition);
measurement.getEcefVelocity(ecefVelocity);
ECEFtoNEDPositionVelocityConverter.convertECEFtoNED(ecefPosition, ecefVelocity, nedPosition, nedVelocity);
final var satLatitude = nedPosition.getLatitude();
final var satLongitude = nedPosition.getLongitude();
userLatitude += satLatitude / numMeasurements;
userLongitude += satLongitude / numMeasurements;
}
nedPosition.setCoordinates(userLatitude, userLongitude, 0.0);
nedVelocity.setCoordinates(0.0, 0.0, 0.0);
NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
priorPositionAndVelocity = new ECEFPositionAndVelocity(ecefPosition, ecefVelocity);
}
/**
* Computes norm of provided coordinates.
*
* @param x x coordinate.
* @param y y coordinate.
* @param z z coordinate.
* @return computed norm.
*/
private static double norm(final double x, final double y, final double z) {
return Math.sqrt(x * x + y * y + z * z);
}
/**
* Computes norm of error between estimated state
* and predicted state.
*
* @return norm of error.
*/
private double predictionError() {
var sqrPredictionError = 0.0;
for (var i = 0; i < STATE_COMPONENTS; i++) {
final var diff = xEst.getElementAtIndex(i) - xPred.getElementAtIndex(i);
sqrPredictionError += diff * diff;
}
return Math.sqrt(sqrPredictionError);
}
}