DynamicOutlierFilter.java
/* Copyright 2002-2022 CS GROUP
* Licensed to CS GROUP (CS) under one or more
* contributor license agreements. See the NOTICE file distributed with
* this work for additional information regarding copyright ownership.
* CS licenses this file to You 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 org.orekit.estimation.measurements.modifiers;
import org.hipparchus.exception.LocalizedCoreFormats;
import org.hipparchus.exception.MathIllegalArgumentException;
import org.hipparchus.util.FastMath;
import org.orekit.estimation.measurements.EstimatedMeasurement;
import org.orekit.estimation.measurements.ObservedMeasurement;
/** Modifier that sets estimated measurement weight to 0 if residual is too far from expected domain.
* The "dynamic" aspect comes from the fact that the value of sigma can be changed on demand.
* This is mainly used when searching for outliers in Kalman filters' prediction phase.
* The value of sigma is then set to the square root of the diagonal of the matrix (H.Ppred.Ht+R)
* Note that in the case of the Kalman filter we use the "iteration" word to represent the number of
* measurements processed by the filter so far.
* @param <T> the type of the measurement
* @author Luc Maisonobe
* @since 9.2
*/
public class DynamicOutlierFilter<T extends ObservedMeasurement<T>> extends OutlierFilter<T> {
/** Current value of sigma. */
private double[] sigma;
/** Simple constructor.
* @param warmup number of iterations before with filter is not applied
* @param maxSigma detection limit for outlier
*/
public DynamicOutlierFilter(final int warmup,
final double maxSigma) {
super(warmup, maxSigma);
this.sigma = null;
}
/** Get the current value of sigma.
* @return The current value of sigma
*/
public double[] getSigma() {
return sigma == null ? null : sigma.clone();
}
/** Set the current value of sigma.
* @param sigma The value of sigma to set
*/
public void setSigma(final double[] sigma) {
this.sigma = sigma == null ? null : sigma.clone();
}
/** {@inheritDoc} */
@Override
public void modify(final EstimatedMeasurement<T> estimated) {
// Do not apply the filter if current iteration/measurement is lower than
// warmup attribute or if the attribute sigma has not been initialized yet
if (estimated.getIteration() > getWarmup() && sigma != null) {
final double[] observed = estimated.getObservedMeasurement().getObservedValue();
final double[] theoretical = estimated.getEstimatedValue();
// Check that the dimension of sigma array is consistent with the measurement
if (observed.length != sigma.length) {
throw new MathIllegalArgumentException(LocalizedCoreFormats.DIMENSIONS_MISMATCH,
sigma.length, getSigma().length);
}
// Check if observed value is not too far from estimation
for (int i = 0; i < observed.length; ++i) {
if (FastMath.abs(observed[i] - theoretical[i]) > getMaxSigma() * sigma[i]) {
// observed value is too far, reject measurement
estimated.setStatus(EstimatedMeasurement.Status.REJECTED);
}
}
}
}
}