GNSSKalmanEpochEstimator.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.navigation.frames.CoordinateTransformation;
import com.irurueta.navigation.geodesic.Constants;
import com.irurueta.units.Time;
import com.irurueta.units.TimeConverter;
import com.irurueta.units.TimeUnit;
import java.util.Collection;
/**
* Implements one cycle of the GNSS extended Kalman filter.
* 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_KF_Epoch.m">
* https://github.com/ymjdz/MATLAB-Codes/blob/master/GNSS_KF_Epoch.m
* </a>
*/
public class GNSSKalmanEpochEstimator {
/**
* 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 rows and columns of transition and system noise covariance matrices.
*/
private static final int MATRIX_SIZE = 8;
/**
* Constructor.
* Prevents instantiation of utility class.
*/
private GNSSKalmanEpochEstimator() {
}
/**
* Estimates the update of Kalman filter state and covariance matrix for a single
* epoch.
*
* @param measurements satellite measurements data.
* @param propagationInterval propagation interval.
* @param previousState previous GNSS estimates and Kalman filter error
* covariance matrix.
* @param config system configuration (usually obtained through
* calibration).
* @return new Kalman filter state.
* @throws AlgebraException if there are numerical instabilities.
*/
public static GNSSKalmanState estimate(
final Collection<GNSSMeasurement> measurements, final Time propagationInterval,
final GNSSKalmanState previousState, final GNSSKalmanConfig config) throws AlgebraException {
return estimate(measurements, convertTime(propagationInterval), previousState, config);
}
/**
* Estimates the update of Kalman filter state and covariance matrix fo a single
* epoch.
*
* @param measurements satellite measurements data.
* @param propagationInterval propagation interval.
* @param previousState previousGNSS estimates and Kalman filter error
* covariance matrix.
* @param config system configuration (usually obtained through
* calibration).
* @param result instance where updated Kalman filter state will be
* stored.
* @throws AlgebraException if there are numerical instabilities.
*/
public static void estimate(final Collection<GNSSMeasurement> measurements,
final Time propagationInterval,
final GNSSKalmanState previousState,
final GNSSKalmanConfig config,
final GNSSKalmanState result) throws AlgebraException {
estimate(measurements, convertTime(propagationInterval), previousState, config, result);
}
/**
* Estimates the update of Kalman filter state and covariance matrix for a single
* epoch.
*
* @param measurements satellite measurements data.
* @param propagationInterval propagation interval.
* @param previousEstimation previousGNSS estimates.
* @param previousCovariance previousKalman filter error covariance matrix.
* @param config system configuration (usually obtained through
* calibration).
* @param updatedEstimation instance where updated GNSS estimate will be stored
* after executing this method.
* @param updatedCovariance instance where updated Kalman filter error covariance
* matrix will be stored.
* @throws IllegalArgumentException if provided previous covariance matrix is not
* 8x8.
* @throws AlgebraException if there are numerical instabilities.
*/
public static void estimate(final Collection<GNSSMeasurement> measurements,
final Time propagationInterval,
final GNSSEstimation previousEstimation,
final Matrix previousCovariance,
final GNSSKalmanConfig config,
final GNSSEstimation updatedEstimation,
final Matrix updatedCovariance) throws AlgebraException {
estimate(measurements, convertTime(propagationInterval), previousEstimation, previousCovariance, config,
updatedEstimation, updatedCovariance);
}
/**
* Estimates the update of Kalman filter state and covariance matrix for a single
* epoch.
*
* @param measurements satellite measurements data.
* @param propagationInterval propagation interval expressed in seconds (s).
* @param previousState previous GNSS estimates and Kalman filter error
* covariance matrix.
* @param config system configuration (usually obtained through
* calibration).
* @return new Kalman filter state.
* @throws AlgebraException if there are numerical instabilities.
*/
public static GNSSKalmanState estimate(
final Collection<GNSSMeasurement> measurements,
final double propagationInterval,
final GNSSKalmanState previousState,
final GNSSKalmanConfig config) throws AlgebraException {
final var result = new GNSSKalmanState();
estimate(measurements, propagationInterval, previousState, config, result);
return result;
}
/**
* Estimates the update of Kalman filter state and covariance matrix for a single
* epoch.
*
* @param measurements satellite measurements data.
* @param propagationInterval propagation interval expressed in seconds (s).
* @param previousState previous GNSS estimates and Kalman filter error
* covariance matrix.
* @param config system configuration (usually obtained through
* calibration).
* @param result instance where updated Kalman filter state will be
* stored.
* @throws AlgebraException if there are numerical instabilities.
*/
public static void estimate(final Collection<GNSSMeasurement> measurements,
final double propagationInterval,
final GNSSKalmanState previousState,
final GNSSKalmanConfig config,
final GNSSKalmanState result) throws AlgebraException {
final var resultEstimation = new GNSSEstimation();
final var resultCovariance = new Matrix(GNSSEstimation.NUM_PARAMETERS, GNSSEstimation.NUM_PARAMETERS);
estimate(measurements, propagationInterval, previousState.getEstimation(), previousState.getCovariance(),
config, resultEstimation, resultCovariance);
result.setEstimation(resultEstimation);
result.setCovariance(resultCovariance);
}
/**
* Estimates the update of Kalman filter state and covariance matrix for a single
* epoch.
*
* @param measurements satellite measurements data.
* @param propagationInterval propagation interval expressed in seconds (s).
* @param previousEstimation previous GNSS estimates.
* @param previousCovariance previous Kalman filter error covariance matrix.
* @param config system configuration (usually obtained through
* calibration).
* @param updatedEstimation instance where updated GNSS estimate will be stored
* after executing this method.
* @param updatedCovariance instance where updated Kalman filter error covariance
* matrix will be stored.
* @throws IllegalArgumentException if provided previous covariance matrix is not
* 8x8.
* @throws AlgebraException if there are numerical instabilities.
*/
@SuppressWarnings("DuplicatedCode")
public static void estimate(final Collection<GNSSMeasurement> measurements,
final double propagationInterval,
final GNSSEstimation previousEstimation,
final Matrix previousCovariance,
final GNSSKalmanConfig config,
final GNSSEstimation updatedEstimation,
final Matrix updatedCovariance) throws AlgebraException {
if (previousCovariance.getRows() != GNSSEstimation.NUM_PARAMETERS
|| previousCovariance.getColumns() != GNSSEstimation.NUM_PARAMETERS) {
throw new IllegalArgumentException();
}
// SYSTEM PROPAGATION PHASE
// 1. Determine transition matrix using (9.147) and (9.150)
final var phiMatrix = Matrix.identity(MATRIX_SIZE, MATRIX_SIZE);
phiMatrix.setElementAt(0, 3, propagationInterval);
phiMatrix.setElementAt(1, 4, propagationInterval);
phiMatrix.setElementAt(2, 5, propagationInterval);
phiMatrix.setElementAt(6, 7, propagationInterval);
// 2. Determine system noise covariance matrix using (9.152)
final var propagationInterval2 = propagationInterval * propagationInterval;
final var propagationInterval3 = propagationInterval2 * propagationInterval;
final var accelerationPSD = config.getAccelerationPSD();
final var clockFrequencyPSD = config.getClockFrequencyPSD();
final var clockPhasePSD = config.getClockPhasePSD();
final var value1 = accelerationPSD * propagationInterval3 / 3.0;
final var value2 = accelerationPSD * propagationInterval2 / 2.0;
final var value3 = accelerationPSD * propagationInterval;
final var value4 = clockFrequencyPSD * propagationInterval3 / 3.0 + clockPhasePSD * propagationInterval;
final var value5 = clockFrequencyPSD * propagationInterval2 / 2.0;
final var value6 = clockFrequencyPSD * propagationInterval;
final var qMatrix = new Matrix(MATRIX_SIZE, MATRIX_SIZE);
qMatrix.setElementAt(0, 0, value1);
qMatrix.setElementAt(1, 1, value1);
qMatrix.setElementAt(2, 2, value1);
qMatrix.setElementAt(0, 3, value2);
qMatrix.setElementAt(1, 4, value2);
qMatrix.setElementAt(2, 5, value2);
qMatrix.setElementAt(3, 0, value2);
qMatrix.setElementAt(4, 1, value2);
qMatrix.setElementAt(5, 2, value2);
qMatrix.setElementAt(3, 3, value3);
qMatrix.setElementAt(4, 4, value3);
qMatrix.setElementAt(5, 5, value3);
qMatrix.setElementAt(6, 6, value4);
qMatrix.setElementAt(6, 7, value5);
qMatrix.setElementAt(7, 6, value5);
qMatrix.setElementAt(7, 7, value6);
// 3. Propagate state estimates using (3.14)
final var xEstOld = previousEstimation.asMatrix();
final var xEstPropagated = phiMatrix.multiplyAndReturnNew(xEstOld);
final var propagatedVelocity = xEstPropagated.getSubmatrix(
3, 0, 5, 0);
final var propagatedPosition = xEstPropagated.getSubmatrix(
0, 0, 2, 0);
// 4. Propagate state estimation error covariance matrix using (3.15)
final var pMatrixPropagated = phiMatrix.multiplyAndReturnNew(previousCovariance);
phiMatrix.transpose();
pMatrixPropagated.multiply(phiMatrix);
pMatrixPropagated.add(qMatrix);
// MEASUREMENT UPDATE PHASE
// Skew symmetric matrix of Earth rate
final var omegaIe = Utils.skewMatrix(new double[]{0.0, 0.0, EARTH_ROTATION_RATE});
final var numberOfMeasurements = measurements.size();
final var uAseT = new Matrix(numberOfMeasurements, 3);
final var predMeas = new Matrix(numberOfMeasurements, 2);
final var cei = Matrix.identity(CoordinateTransformation.ROWS, CoordinateTransformation.COLS);
final var satellitePosition = new Matrix(CoordinateTransformation.ROWS, 1);
final var satelliteVelocity = new Matrix(CoordinateTransformation.ROWS, 1);
final var deltaR = new Matrix(CoordinateTransformation.ROWS, 1);
final var tmp1 = new Matrix(CoordinateTransformation.ROWS, 1);
final var tmp2 = new Matrix(CoordinateTransformation.ROWS, 1);
final var tmp3 = new Matrix(CoordinateTransformation.ROWS, 1);
final var tmp4 = new Matrix(CoordinateTransformation.ROWS, 1);
final var tmp5 = new Matrix(CoordinateTransformation.ROWS, 1);
final var tmp6 = new Matrix(CoordinateTransformation.ROWS, 1);
final var tmp7 = new Matrix(1, CoordinateTransformation.ROWS);
// 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();
final var deltaX = measX - xEstPropagated.getElementAtIndex(0);
final var deltaY = measY - xEstPropagated.getElementAtIndex(1);
final var deltaZ = measZ - xEstPropagated.getElementAtIndex(2);
final var approxRange = Math.sqrt(deltaX * deltaX + deltaY * deltaY + deltaZ * deltaZ);
// 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.165)
satellitePosition.setElementAtIndex(0, measX);
satellitePosition.setElementAtIndex(1, measY);
satellitePosition.setElementAtIndex(2, measZ);
cei.multiply(satellitePosition, deltaR);
for (var i = 0; i < CoordinateTransformation.ROWS; i++) {
deltaR.setElementAtIndex(i, deltaR.getElementAtIndex(i) - xEstPropagated.getElementAtIndex(i));
}
final var range = Utils.normF(deltaR);
predMeas.setElementAt(j, 0, range + xEstPropagated.getElementAtIndex(6));
// Predict line of sight
for (var i = 0; i < CoordinateTransformation.ROWS; i++) {
uAseT.setElementAt(j, i, deltaR.getElementAtIndex(i) / range);
}
// Predict pseudo-range rate using (9.165)
satelliteVelocity.setElementAtIndex(0, measurement.getVx());
satelliteVelocity.setElementAtIndex(1, measurement.getVy());
satelliteVelocity.setElementAtIndex(2, measurement.getVz());
omegaIe.multiply(satellitePosition, tmp1);
satelliteVelocity.add(tmp1, tmp2);
cei.multiply(tmp2, tmp3);
omegaIe.multiply(propagatedPosition, tmp4);
propagatedVelocity.add(tmp4, tmp6);
tmp3.subtract(tmp6, tmp5);
uAseT.getSubmatrix(j, 0, j, 2, tmp7);
final var rangeRate = Utils.dotProduct(tmp7, tmp5);
predMeas.setElementAt(j, 1, rangeRate + xEstPropagated.getElementAtIndex(7));
j++;
}
// 5. Set-up measurement matrix using (9.163)
final var h = new Matrix(2 * numberOfMeasurements, GNSSEstimation.NUM_PARAMETERS);
for (int j1 = 0, j2 = numberOfMeasurements; j1 < numberOfMeasurements; j1++, j2++) {
for (int i1 = 0, i2 = 3; i1 < CoordinateTransformation.ROWS; i1++, i2++) {
final var value = -uAseT.getElementAt(j1, i1);
h.setElementAt(j1, i1, value);
h.setElementAt(j2, i2, value);
}
h.setElementAt(j1, 6, 1.0);
h.setElementAt(j2, 7, 1.0);
}
// 6. Set-up measurement noise covariance matrix assuming all measurements are independent
// and have equal variance for a given measurement type
final var pseudoRangeSD = config.getPseudoRangeSD();
final var pseudoRangeSD2 = pseudoRangeSD * pseudoRangeSD;
final var rangeRateSD = config.getRangeRateSD();
final var rangeRateSD2 = rangeRateSD * rangeRateSD;
final var r = new Matrix(2 * numberOfMeasurements, 2 * numberOfMeasurements);
for (int i1 = 0, i2 = numberOfMeasurements; i1 < numberOfMeasurements; i1++, i2++) {
r.setElementAt(i1, i1, pseudoRangeSD2);
r.setElementAt(i2, i2, rangeRateSD2);
}
// 7. Calculate Kalman gain using (3.21)
final var hTransposed = h.transposeAndReturnNew();
final var tmp8 = h.multiplyAndReturnNew(pMatrixPropagated.multiplyAndReturnNew(hTransposed));
tmp8.add(r);
final var tmp9 = Utils.inverse(tmp8);
final var k = pMatrixPropagated.multiplyAndReturnNew(hTransposed);
k.multiply(tmp9);
// 8. Formulate measurement innovations using (3.88)
final var deltaZ = new Matrix(2 * numberOfMeasurements, 1);
var i1 = 0;
var i2 = numberOfMeasurements;
for (final var measurement : measurements) {
deltaZ.setElementAtIndex(i1, measurement.getPseudoRange() - predMeas.getElementAt(i1, 0));
deltaZ.setElementAtIndex(i2, measurement.getPseudoRate() - predMeas.getElementAt(i1, 1));
i1++;
i2++;
}
// 9. Update state estimates using (3.24)
xEstPropagated.add(k.multiplyAndReturnNew(deltaZ));
// xEstPropagated now contains updated state
updatedEstimation.fromMatrix(xEstPropagated);
// 10. Update state estimation error covariance matrix using (3.25)
if (updatedCovariance.getRows() != GNSSEstimation.NUM_PARAMETERS
|| updatedCovariance.getColumns() != GNSSEstimation.NUM_PARAMETERS) {
updatedCovariance.resize(GNSSEstimation.NUM_PARAMETERS, GNSSEstimation.NUM_PARAMETERS);
}
Matrix.identity(updatedCovariance);
k.multiply(h);
updatedCovariance.subtract(k);
updatedCovariance.multiply(pMatrixPropagated);
}
/**
* Converts time instance into a value expressed in seconds.
*
* @param time time instance to be converted.
* @return time value expressed in seconds.
*/
private static double convertTime(final Time time) {
return TimeConverter.convert(time.getValue().doubleValue(), time.getUnit(), TimeUnit.SECOND);
}
}