View Javadoc
1   /*
2    * Licensed to the Hipparchus project under one or more
3    * contributor license agreements.  See the NOTICE file distributed with
4    * this work for additional information regarding copyright ownership.
5    * The Hipparchus project 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    *      https://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  
18  package org.hipparchus.filtering.kalman.extended;
19  
20  import org.hipparchus.exception.MathRuntimeException;
21  import org.hipparchus.filtering.kalman.AbstractKalmanFilter;
22  import org.hipparchus.filtering.kalman.Measurement;
23  import org.hipparchus.filtering.kalman.ProcessEstimate;
24  import org.hipparchus.linear.MatrixDecomposer;
25  import org.hipparchus.linear.RealMatrix;
26  import org.hipparchus.linear.RealVector;
27  
28  /**
29   * Kalman filter for {@link NonLinearProcess non-linear process}.
30   * @param <T> the type of the measurements
31   * @since 1.3
32   */
33  public class ExtendedKalmanFilter<T extends Measurement> extends AbstractKalmanFilter<T> {
34  
35      /** Process to be estimated. */
36      private final NonLinearProcess<T> process;
37  
38      /** Simple constructor.
39       * @param decomposer decomposer to use for the correction phase
40       * @param process non-linear process to estimate
41       * @param initialState initial state
42       */
43      public ExtendedKalmanFilter(final MatrixDecomposer decomposer,
44                                  final NonLinearProcess<T> process,
45                                  final ProcessEstimate initialState) {
46          super(decomposer, initialState);
47          this.process = process;
48      }
49  
50      /** {@inheritDoc} */
51      @Override
52      public ProcessEstimate estimationStep(final T measurement)
53          throws MathRuntimeException {
54  
55          // prediction phase
56          final NonLinearEvolution evolution = process.getEvolution(getCorrected().getTime(),
57                                                                    getCorrected().getState(),
58                                                                    measurement);
59  
60          final RealMatrix stm = evolution.getStateTransitionMatrix();
61          predict(evolution.getCurrentTime(), evolution.getCurrentState(),
62                  stm, evolution.getProcessNoiseMatrix());
63  
64          // correction phase
65          final RealMatrix h          = evolution.getMeasurementJacobian();
66          final RealMatrix s          = computeInnovationCovarianceMatrix(measurement.getCovariance(), h);
67          final RealVector innovation = (h == null) ? null : process.getInnovation(measurement, evolution, s);
68          correct(measurement, stm, innovation, h, s);
69  
70          if (getObserver() != null) {
71              getObserver().updatePerformed(this);
72          }
73  
74          return getCorrected();
75  
76      }
77  
78  }