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;
19
20 import org.hipparchus.exception.MathIllegalArgumentException;
21 import org.hipparchus.linear.MatrixDecomposer;
22 import org.hipparchus.linear.RealMatrix;
23 import org.hipparchus.linear.RealVector;
24
25 /**
26 * Shared parts between linear and non-linear Kalman filters.
27 * @param <T> the type of the measurements
28 * @since 1.3
29 */
30 public abstract class AbstractKalmanFilter<T extends Measurement> implements KalmanFilter<T> {
31
32 /** Decomposer decomposer to use for the correction phase. */
33 private final MatrixDecomposer decomposer;
34
35 /** Predicted state. */
36 private ProcessEstimate predicted;
37
38 /** Corrected state. */
39 private ProcessEstimate corrected;
40
41 /** Simple constructor.
42 * @param decomposer decomposer to use for the correction phase
43 * @param initialState initial state
44 */
45 protected AbstractKalmanFilter(final MatrixDecomposer decomposer, final ProcessEstimate initialState) {
46 this.decomposer = decomposer;
47 this.corrected = initialState;
48 }
49
50 /** Perform prediction step.
51 * @param time process time
52 * @param predictedState predicted state vector
53 * @param stm state transition matrix
54 * @param noise process noise covariance matrix
55 */
56 protected void predict(final double time, final RealVector predictedState, final RealMatrix stm, final RealMatrix noise) {
57 final RealMatrix predictedCovariance =
58 stm.multiply(corrected.getCovariance().multiplyTransposed(stm)).add(noise);
59 predicted = new ProcessEstimate(time, predictedState, predictedCovariance);
60 corrected = null;
61 }
62
63 /** Compute innovation covariance matrix.
64 * @param r measurement covariance
65 * @param h Jacobian of the measurement with respect to the state
66 * (may be null if measurement should be ignored)
67 * @return innovation covariance matrix, defined as \(h.P.h^T + r\), or
68 * null if h is null
69 */
70 protected RealMatrix computeInnovationCovarianceMatrix(final RealMatrix r, final RealMatrix h) {
71 if (h == null) {
72 return null;
73 }
74 final RealMatrix phT = predicted.getCovariance().multiplyTransposed(h);
75 return h.multiply(phT).add(r);
76 }
77
78 /** Perform correction step.
79 * @param measurement single measurement to handle
80 * @param stm state transition matrix
81 * @param innovation innovation vector (i.e. residuals)
82 * (may be null if measurement should be ignored)
83 * @param h Jacobian of the measurement with respect to the state
84 * (may be null if measurement should be ignored)
85 * @param s innovation covariance matrix
86 * (may be null if measurement should be ignored)
87 * @exception MathIllegalArgumentException if matrix cannot be decomposed
88 */
89 protected void correct(final T measurement, final RealMatrix stm, final RealVector innovation,
90 final RealMatrix h, final RealMatrix s)
91 throws MathIllegalArgumentException {
92
93 if (innovation == null) {
94 // measurement should be ignored
95 corrected = predicted;
96 return;
97 }
98
99 // compute Kalman gain k
100 // the following is equivalent to k = p.h^T * (h.p.h^T + r)^(-1)
101 // we don't want to compute the inverse of a matrix,
102 // we start by post-multiplying by h.p.h^T + r and get
103 // k.(h.p.h^T + r) = p.h^T
104 // then we transpose, knowing that both p and r are symmetric matrices
105 // (h.p.h^T + r).k^T = h.p
106 // then we can use linear system solving instead of matrix inversion
107 final RealMatrix k = decomposer.
108 decompose(s).
109 solve(h.multiply(predicted.getCovariance())).
110 transpose();
111
112 // correct state vector
113 final RealVector correctedState = predicted.getState().add(k.operate(innovation));
114
115 // here we use the Joseph algorithm (see "Fundamentals of Astrodynamics and Applications,
116 // Vallado, Fourth Edition ยง10.6 eq.10-34) which is equivalent to
117 // the traditional Pest = (I - k.h) x Ppred expression but guarantees the output stays symmetric:
118 // Pest = (I -k.h) Ppred (I - k.h)^T + k.r.k^T
119 final RealMatrix idMkh = k.multiply(h);
120 for (int i = 0; i < idMkh.getRowDimension(); ++i) {
121 for (int j = 0; j < idMkh.getColumnDimension(); ++j) {
122 idMkh.multiplyEntry(i, j, -1);
123 }
124 idMkh.addToEntry(i, i, 1.0);
125 }
126 final RealMatrix r = measurement.getCovariance();
127 final RealMatrix correctedCovariance =
128 idMkh.multiply(predicted.getCovariance()).multiplyTransposed(idMkh).
129 add(k.multiply(r).multiplyTransposed(k));
130
131 corrected = new ProcessEstimate(measurement.getTime(), correctedState, correctedCovariance,
132 stm, h, s, k);
133
134 }
135
136 /** Get the predicted state.
137 * @return predicted state
138 */
139 @Override
140 public ProcessEstimate getPredicted() {
141 return predicted;
142 }
143
144 /** Get the corrected state.
145 * @return corrected state
146 */
147 @Override
148 public ProcessEstimate getCorrected() {
149 return corrected;
150 }
151
152 }