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 return getCorrected();
70
71 }
72
73 }