GNSSKalmanFilteredEstimator.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.navigation.LockedException;
import com.irurueta.navigation.NotReadyException;
import com.irurueta.units.Time;
import com.irurueta.units.TimeConverter;
import com.irurueta.units.TimeUnit;
import java.util.ArrayList;
import java.util.Collection;
/**
* Calculates position, velocity, clock offset and clock drift using an
* unweighted iterated least squares estimator along with a Kalman filter
* to smooth results.
* 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_Kalman_Filter.m">
* https://github.com/ymjdz/MATLAB-Codes/blob/master/GNSS_Kalman_Filter.m
* </a>
*/
public class GNSSKalmanFilteredEstimator {
/**
* Internal estimator to compute least squares solution for GNSS measurements.
*/
private final GNSSLeastSquaresPositionAndVelocityEstimator lsEstimator =
new GNSSLeastSquaresPositionAndVelocityEstimator();
/**
* Listener to notify events raised by this instance.
*/
private GNSSKalmanFilteredEstimatorListener listener;
/**
* Minimum epoch interval expressed in seconds (s) between consecutive
* propagations or measurements.
* Attempting to propagate results using Kalman filter or updating measurements when
* intervals are lass than this value, will be ignored.
*/
private double epochInterval;
/**
* GNSS Kalman filter configuration parameters (usually obtained through calibration).
*/
private GNSSKalmanConfig config;
/**
* GNSS measurements of a collection of satellites.
*/
private Collection<GNSSMeasurement> measurements;
/**
* Current estimation containing user ECEF position, user ECEF velocity, clock offset
* and clock drift.
*/
private GNSSEstimation estimation;
/**
* Current Kalman filter state containing current GNSS estimation along with
* Kalman filter covariance error matrix.
*/
private GNSSKalmanState state;
/**
* Timestamp expressed in seconds since epoch time when Kalman filter state
* was last propagated.
*/
private Double lastStateTimestamp;
/**
* Indicates whether this estimator is running or not.
*/
private boolean running;
/**
* Constructor.
*/
public GNSSKalmanFilteredEstimator() {
}
/**
* Constructor.
*
* @param config GNSS Kalman filter configuration parameters (usually obtained
* through calibration).
*/
public GNSSKalmanFilteredEstimator(final GNSSKalmanConfig config) {
this.config = new GNSSKalmanConfig(config);
}
/**
* Constructor.
*
* @param epochInterval minimum epoch interval expressed in seconds (s) between
* consecutive propagations or measurements.
* @throws IllegalArgumentException if provided epoch interval is negative.
*/
public GNSSKalmanFilteredEstimator(final double epochInterval) {
try {
setEpochInterval(epochInterval);
} catch (final LockedException ignore) {
// never happens
}
}
/**
* Constructor.
*
* @param listener listener to notify events raised by this instance.
*/
public GNSSKalmanFilteredEstimator(final GNSSKalmanFilteredEstimatorListener listener) {
this.listener = listener;
}
/**
* Constructor.
*
* @param config GNSS Kalman filter configuration parameters (usually
* obtained through calibration).
* @param epochInterval minimum epoch interval expressed in seconds (s) between
* consecutive propagations or measurements.
* @throws IllegalArgumentException if provided epoch interval is negative.
*/
public GNSSKalmanFilteredEstimator(final GNSSKalmanConfig config, final double epochInterval) {
this(epochInterval);
this.config = new GNSSKalmanConfig(config);
}
/**
* Constructor.
*
* @param config GNSS Kalman filter configuration parameters (usually
* obtained through calibration).
* @param listener listener to notify events raised by this instance.
*/
public GNSSKalmanFilteredEstimator(
final GNSSKalmanConfig config, final GNSSKalmanFilteredEstimatorListener listener) {
this(config);
this.listener = listener;
}
/**
* Constructor.
*
* @param epochInterval minimum epoch interval expressed in seconds (s) between
* consecutive propagations or measurements.
* @param listener listener to notify events raised by this instance.
* @throws IllegalArgumentException if provided epoch interval is negative.
*/
public GNSSKalmanFilteredEstimator(
final double epochInterval, final GNSSKalmanFilteredEstimatorListener listener) {
this(epochInterval);
this.listener = listener;
}
/**
* Constructor.
*
* @param config GNSS Kalman filter configuration parameters (usually obtained
* through calibration).
* @param epochInterval minimum epoch interval expressed in seconds (s) between
* consecutive propagations or measurements.
* @param listener listener to notify events raised by this instance.
* @throws IllegalArgumentException if provided epoch interval is negative.
*/
public GNSSKalmanFilteredEstimator(
final GNSSKalmanConfig config, final double epochInterval,
final GNSSKalmanFilteredEstimatorListener listener) {
this(config, epochInterval);
this.listener = listener;
}
/**
* Constructor.
*
* @param epochInterval minimum epoch interval between consecutive
* propagations or measurements.
* @throws IllegalArgumentException if provided epoch interval is negative.
*/
public GNSSKalmanFilteredEstimator(final Time epochInterval) {
this(TimeConverter.convert(epochInterval.getValue().doubleValue(), epochInterval.getUnit(), TimeUnit.SECOND));
}
/**
* Constructor.
*
* @param config GNSS Kalman filter configuration parameters (usually
* obtained through calibration).
* @param epochInterval minimum epoch interval between consecutive propagations
* or measurements.
* @throws IllegalArgumentException if provided epoch interval is negative.
*/
public GNSSKalmanFilteredEstimator(final GNSSKalmanConfig config, final Time epochInterval) {
this(config, TimeConverter.convert(epochInterval.getValue().doubleValue(), epochInterval.getUnit(),
TimeUnit.SECOND));
}
/**
* Constructor.
*
* @param epochInterval minimum epoch interval between consecutive propagations
* or measurements.
* @param listener listener to notify events raised by this instance.
* @throws IllegalArgumentException if provided epoch interval is negative.
*/
public GNSSKalmanFilteredEstimator(final Time epochInterval, final GNSSKalmanFilteredEstimatorListener listener) {
this(epochInterval);
this.listener = listener;
}
/**
* Constructor.
*
* @param config GNSS Kalman filter configuration parameters (usually
* obtained through calibration).
* @param epochInterval minimum epoch interval between consecutive propagations
* or measurements.
* @param listener listener to notify events raised by this instance.
* @throws IllegalArgumentException if provided epoch interval is negative.
*/
public GNSSKalmanFilteredEstimator(
final GNSSKalmanConfig config, final Time epochInterval,
final GNSSKalmanFilteredEstimatorListener listener) {
this(config, epochInterval);
this.listener = listener;
}
/**
* Gets listener to notify events raised by this instance.
*
* @return listener to notify events raised by this instance.
*/
public GNSSKalmanFilteredEstimatorListener 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 GNSSKalmanFilteredEstimatorListener listener) throws LockedException {
if (running) {
throw new LockedException();
}
this.listener = listener;
}
/**
* Gets minimum epoch interval expressed in seconds (s) between consecutive
* propagations or measurements expressed in seconds.
* Attempting to propagate results using Kalman filter or updating measurements
* when intervals are less than this value, will be ignored.
*
* @return minimum epoch interval between consecutive propagations or
* measurements.
*/
public double getEpochInterval() {
return epochInterval;
}
/**
* Sets minimum epoch interval expressed in seconds (s) between consecutive
* propagations or measurements expressed in seconds.
* Attempting to propagate results using Kalman filter or updating measurements
* when intervals are less than this value, will be ignored.
*
* @param epochInterval minimum epoch interval expressed in seconds (s) between
* consecutive propagations or measurements.
* @throws LockedException if this estimator is already running.
* @throws IllegalArgumentException if provided epoch interval is negative.
*/
public void setEpochInterval(final double epochInterval) throws LockedException {
if (running) {
throw new LockedException();
}
if (epochInterval < 0.0) {
throw new IllegalArgumentException();
}
this.epochInterval = epochInterval;
}
/**
* Gets minimum epoch interval between consecutive propagations or measurements.
* Attempting to propagate results using Kalman filter or updating measurements
* when intervals are less than this value, will be ignored.
*
* @param result instance where minimum epoch interval will be stored.
*/
public void getEpochIntervalAsTime(final Time result) {
result.setValue(epochInterval);
result.setUnit(TimeUnit.SECOND);
}
/**
* Gets minimum epoch interval between consecutive propagations or measurements.
* Attempting to propagate results using Kalman filter or updating measurements
* when intervals are less than this value, will be ignored.
*
* @return minimum epoch interval.
*/
public Time getEpochIntervalAsTime() {
return new Time(epochInterval, TimeUnit.SECOND);
}
/**
* Sets minimum epoch interval between consecutive propagations or measurements.
* Attempting to propagate results using Kalman filter or updating measurements
* when intervals are less than this value, will be ignored.
*
* @param epochInterval minimum epoch interval.
* @throws LockedException if this estimator is already running.
* @throws IllegalArgumentException if provided epoch interval is negative.
*/
public void setEpochInterval(final Time epochInterval) throws LockedException {
final var epochIntervalSeconds = TimeConverter.convert(epochInterval.getValue().doubleValue(),
epochInterval.getUnit(), TimeUnit.SECOND);
setEpochInterval(epochIntervalSeconds);
}
/**
* Gets GNSS Kalman configuration parameters (usually obtained through
* calibration).
*
* @param result instance where GNSS Kalman configuration parameters will be
* stored.
* @return true if result instance is updated, false otherwise.
*/
public boolean getConfig(final GNSSKalmanConfig result) {
if (config != null) {
result.copyFrom(config);
return true;
} else {
return false;
}
}
/**
* Gets GNSS Kalman configuration parameters (usually obtained through
* calibration).
*
* @return GNSS Kalman configuration parameters.
*/
public GNSSKalmanConfig getConfig() {
return config;
}
/**
* Sets GNSS Kalman configuration parameters (usually obtained through
* calibration).
*
* @param config GNSS Kalman configuration parameters to be set.
* @throws LockedException if this estimator is already running.
*/
public void setConfig(final GNSSKalmanConfig config) throws LockedException {
if (running) {
throw new LockedException();
}
this.config = new GNSSKalmanConfig(config);
}
/**
* Gets last updated GNSS measurements of a collection of satellites.
*
* @return last updated GNSS measurements of a collection of satellites.
*/
public Collection<GNSSMeasurement> getMeasurements() {
if (measurements == null) {
return null;
}
final var result = new ArrayList<GNSSMeasurement>();
for (final var measurement : measurements) {
result.add(new GNSSMeasurement(measurement));
}
return result;
}
/**
* Gets current estimation containing user ECEF position, user ECEF velocity,
* clock offset and clock drift.
*
* @return current estimation containing user ECEF position, user ECEF velocity,
* clock offset and clock drift.
*/
public GNSSEstimation getEstimation() {
return estimation != null ? new GNSSEstimation(estimation) : null;
}
/**
* Gets current estimation containing user ECEF position, user ECEF velocity,
* clock offset and clock drift.
* This method does not update result instance if no estimation is available.
*
* @param result instance where estimation will be stored.
* @return true if result estimation was updated, false otherwise.
*/
public boolean getEstimation(final GNSSEstimation result) {
if (estimation != null) {
result.copyFrom(estimation);
return true;
} else {
return false;
}
}
/**
* Gets current Kalman filter state containing current GNSS estimation along with
* Kalman filter covariance error matrix.
*
* @return current Kalman filter state containing current GNSS estimation along
* with Kalman filter covariance error matrix.
*/
public GNSSKalmanState getState() {
return state != null ? new GNSSKalmanState(state) : null;
}
/**
* Gets current Kalman filter state containing current GNSS estimation along with
* Kalman filter covariance error matrix.
* This method does not update result instance if no state is available.
*
* @param result instance where state will be stored.
* @return true if result state was updated, false otherwise.
*/
public boolean getState(final GNSSKalmanState result) {
if (state != null) {
result.copyFrom(state);
return true;
} else {
return false;
}
}
/**
* Gets timestamp expressed in seconds since epoch time when Kalman filter state
* was last propagated.
*
* @return timestamp expressed in seconds since epoch time when Kalman filter
* state was last propagated.
*/
public Double getLastStateTimestamp() {
return lastStateTimestamp;
}
/**
* Gets timestamp since epoch time when Kalman filter state was last propagated.
*
* @param result instance where timestamp since epoch time when Kalman filter
* state was last propagated will be stored.
* @return true if result instance is updated, false otherwise.
*/
public boolean getLastStateTimestampAsTime(final Time result) {
if (lastStateTimestamp != null) {
result.setValue(lastStateTimestamp);
result.setUnit(TimeUnit.SECOND);
return true;
} else {
return false;
}
}
/**
* Gets timestamp since epoch time when Kalman filter state was last propagated.
*
* @return timestamp since epoch time when Kalman filter state was last
* propagated.
*/
public Time getLastStateTimestampAsTime() {
return lastStateTimestamp != null ? new Time(lastStateTimestamp, TimeUnit.SECOND) : null;
}
/**
* Indicates whether this estimator is running or not.
*
* @return true if this estimator is running, false otherwise.
*/
public boolean isRunning() {
return running;
}
/**
* Indicates whether provided measurements are ready to
* be used for an update.
*
* @param measurements measurements to be checked.
* @return true if estimator is ready, false otherwise.
*/
public static boolean isUpdateMeasurementsReady(final Collection<GNSSMeasurement> measurements) {
return GNSSLeastSquaresPositionAndVelocityEstimator.isValidMeasurements(measurements);
}
/**
* Updates GNSS measurements of this estimator when new satellite measurements
* are available.
* Calls to this method will be ignored if interval between provided timestamp
* and last timestamp when Kalman filter was updated is less than epoch interval.
*
* @param measurements GNSS measurements to be updated.
* @param timestamp timestamp since epoch time when GNSS measurements were
* updated.
* @return true if measurements were updated, false otherwise.
* @throws LockedException if this estimator is already running.
* @throws NotReadyException if estimator is not ready for measurements updates.
* @throws GNSSException if estimation fails due to numerical instabilities.
*/
public boolean updateMeasurements(
final Collection<GNSSMeasurement> measurements, final Time timestamp)
throws LockedException, NotReadyException, GNSSException {
return updateMeasurements(measurements, TimeConverter.convert(
timestamp.getValue().doubleValue(), timestamp.getUnit(), TimeUnit.SECOND));
}
/**
* Updates GNSS measurements of this estimator when new satellite measurements
* are available.
* Call to this method will be ignored if interval between provided timestamp
* and last timestamp when Kalman filter was updated is less than epoch interval.
*
* @param measurements GNSS measurements to be updated.
* @param timestamp timestamp expressed in seconds since epoch time when
* GNSS measurements were updated.
* @return true if measurements were updated, false otherwise.
* @throws LockedException if this estimator is already running.
* @throws NotReadyException if estimator is not ready for measurements updates.
* @throws GNSSException if estimation fails due to numerical instabilities.
*/
public boolean updateMeasurements(
final Collection<GNSSMeasurement> measurements, final double timestamp)
throws LockedException, NotReadyException, GNSSException {
if (running) {
throw new LockedException();
}
if (!isUpdateMeasurementsReady(measurements)) {
throw new NotReadyException();
}
if (lastStateTimestamp != null && timestamp - lastStateTimestamp <= epochInterval) {
return false;
}
try {
running = true;
if (listener != null) {
listener.onUpdateStart(this);
}
this.measurements = new ArrayList<>(measurements);
lsEstimator.setMeasurements(this.measurements);
lsEstimator.setPriorPositionAndVelocityFromEstimation(estimation);
if (estimation != null) {
lsEstimator.estimate(estimation);
} else {
estimation = lsEstimator.estimate();
}
if (listener != null) {
listener.onUpdateEnd(this);
}
} finally {
running = false;
}
propagate(timestamp);
return true;
}
/**
* Indicates whether this estimator is ready for state propagations.
*
* @return true if estimator is ready, false otherwise.
*/
public boolean isPropagateReady() {
return config != null && estimation != null;
}
/**
* Propagates Kalman filter state held by this estimator at provided
* timestamp.
* Call to this method will be ignored if interval between provided timestamp
* and last timestamp when Kalman filter was updated is less than epoch interval.
*
* @param timestamp timestamp since epoch to propagate state.
* @return true if state was propagated, false otherwise.
* @throws LockedException if this estimator is already running.
* @throws NotReadyException if estimator is not ready for measurements updates.
* @throws GNSSException if estimation fails due to numerical instabilities.
*/
public boolean propagate(final Time timestamp) throws LockedException, NotReadyException, GNSSException {
return propagate(TimeConverter.convert(timestamp.getValue().doubleValue(), timestamp.getUnit(),
TimeUnit.SECOND));
}
/**
* Propagates Kalman filter state held by this estimator at provided
* timestamp.
* Call to this method will be ignored if interval between provided timestamp
* and last timestamp when Kalman filter was updated is less than epoch interval.
*
* @param timestamp timestamp expressed in seconds since epoch to propagate state.
* @return true if state was propagated, false otherwise.
* @throws LockedException if this estimator is already running.
* @throws NotReadyException if estimator is not ready for measurements updates.
* @throws GNSSException if estimation fails due to numerical instabilities.
*/
public boolean propagate(final double timestamp) throws LockedException, NotReadyException, GNSSException {
if (running) {
throw new LockedException();
}
if (!isPropagateReady()) {
throw new NotReadyException();
}
final var propagationInterval = lastStateTimestamp != null ? timestamp - lastStateTimestamp : 0.0;
if (lastStateTimestamp != null && propagationInterval <= epochInterval) {
return false;
}
try {
running = true;
if (listener != null) {
listener.onPropagateStart(this);
}
if (state == null) {
state = GNSSKalmanInitializer.initialize(estimation, config);
}
GNSSKalmanEpochEstimator.estimate(measurements, propagationInterval, state, config, state);
lastStateTimestamp = timestamp;
state.getEstimation(estimation);
if (listener != null) {
listener.onPropagateEnd(this);
}
} catch (final AlgebraException e) {
throw new GNSSException(e);
} finally {
running = false;
}
return true;
}
/**
* Resets this estimator.
*
* @throws LockedException if this estimator is already running.
*/
public void reset() throws LockedException {
if (running) {
throw new LockedException();
}
running = true;
measurements = null;
estimation = null;
state = null;
lastStateTimestamp = null;
if (listener != null) {
listener.onReset(this);
}
running = false;
}
}