1 /* Copyright 2002-2017 CS Systèmes d'Information
2 * Licensed to CS Systèmes d'Information (CS) under one or more
3 * contributor license agreements. See the NOTICE file distributed with
4 * this work for additional information regarding copyright ownership.
5 * CS licenses this file to You under the Apache License, Version 2.0
6 * (the "License"); you may not use this file except in compliance with
7 * the License. You may obtain a copy of the License at
8 *
9 * http://www.apache.org/licenses/LICENSE-2.0
10 *
11 * Unless required by applicable law or agreed to in writing, software
12 * distributed under the License is distributed on an "AS IS" BASIS,
13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 * See the License for the specific language governing permissions and
15 * limitations under the License.
16 */
17 package org.orekit.estimation.measurements.modifiers;
18
19 import org.hipparchus.exception.LocalizedCoreFormats;
20 import org.hipparchus.exception.MathIllegalArgumentException;
21 import org.hipparchus.util.FastMath;
22 import org.orekit.estimation.measurements.EstimatedMeasurement;
23 import org.orekit.estimation.measurements.ObservedMeasurement;
24
25 /** Modifier that sets estimated measurement weight to 0 if residual is too far from expected domain.
26 * The "dynamic" aspect comes from the fact that the value of sigma can be changed on demand.
27 * This is mainly used when searching for outliers in Kalman filters' prediction phase.
28 * The value of sigma is then set to the square root of the diagonal of the matrix (H.Ppred.Ht+R)
29 * Note that in the case of the Kalman filter we use the "iteration" word to represent the number of
30 * measurements processed by the filter so far.
31 * @param <T> the type of the measurement
32 * @author Luc Maisonobe
33 * @since 9.2
34 */
35 public class DynamicOutlierFilter<T extends ObservedMeasurement<T>> extends OutlierFilter<T>
36 {
37 /** Current value of sigma. */
38 private double[] sigma;
39
40 /** Simple constructor.
41 * @param warmup number of iterations before with filter is not applied
42 * @param maxSigma detection limit for outlier
43 */
44 public DynamicOutlierFilter(final int warmup,
45 final double maxSigma) {
46 super(warmup, maxSigma);
47 this.sigma = null;
48 }
49
50 /** Get the current value of sigma.
51 * @return The current value of sigma
52 */
53 public double[] getSigma() {
54 return sigma;
55 }
56
57 /** Set the current value of sigma.
58 * @param sigma The value of sigma to set
59 * @throws MathIllegalArgumentException if the size of sigma as input does not match the size of the measurement
60 */
61 public void setSigma(final double[] sigma) {
62 this.sigma = sigma;
63 }
64
65 /** {@inheritDoc} */
66 @Override
67 public void modify(final EstimatedMeasurement<T> estimated) {
68
69 // Do not apply the filter if current iteration/measurement is lower than
70 // warmup attribute or if the attribute sigma has not been initialized yet
71 if ((estimated.getIteration() > getWarmup()) && (sigma != null)) {
72
73 final double[] observed = estimated.getObservedMeasurement().getObservedValue();
74 final double[] theoretical = estimated.getEstimatedValue();
75
76 // Check that the dimension of sigma array is consistent with the measurement
77 if (observed.length != sigma.length) {
78 throw new MathIllegalArgumentException(LocalizedCoreFormats.DIMENSIONS_MISMATCH,
79 sigma.length, getSigma().length);
80 }
81
82 // Check if observed value is not too far from estimation
83 for (int i = 0; i < observed.length; ++i) {
84 if (FastMath.abs(observed[i] - theoretical[i]) > getMaxSigma() * sigma[i]) {
85 // observed value is too far, reject measurement
86 estimated.setStatus(EstimatedMeasurement.Status.REJECTED);
87 }
88 }
89 }
90
91 }
92 }