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 /** Prior corrected covariance. */
42 private RealMatrix priorCovariance;
43
44 /** State transition matrix. */
45 private RealMatrix stateTransitionMatrix;
46
47 /** Observer. */
48 private KalmanObserver observer;
49
50
51 /** Simple constructor.
52 * @param decomposer decomposer to use for the correction phase
53 * @param initialState initial state
54 */
55 protected AbstractKalmanFilter(final MatrixDecomposer decomposer, final ProcessEstimate initialState) {
56 this.decomposer = decomposer;
57 this.corrected = initialState;
58 this.priorCovariance = null;
59 this.stateTransitionMatrix = null;
60 this.observer = null;
61 }
62
63 /** Perform prediction step.
64 * @param time process time
65 * @param predictedState predicted state vector
66 * @param stm state transition matrix
67 * @param noise process noise covariance matrix
68 */
69 protected void predict(final double time, final RealVector predictedState, final RealMatrix stm, final RealMatrix noise) {
70 final RealMatrix predictedCovariance =
71 stm.multiply(corrected.getCovariance().multiplyTransposed(stm)).add(noise);
72 predicted = new ProcessEstimate(time, predictedState, predictedCovariance);
73 stateTransitionMatrix = stm;
74 priorCovariance = corrected.getCovariance();
75 corrected = null;
76 }
77
78 /** Compute innovation covariance matrix.
79 * @param r measurement covariance
80 * @param h Jacobian of the measurement with respect to the state
81 * (may be null if measurement should be ignored)
82 * @return innovation covariance matrix, defined as \(h.P.h^T + r\), or
83 * null if h is null
84 */
85 protected RealMatrix computeInnovationCovarianceMatrix(final RealMatrix r, final RealMatrix h) {
86 if (h == null) {
87 return null;
88 }
89 final RealMatrix phT = predicted.getCovariance().multiplyTransposed(h);
90 return h.multiply(phT).add(r);
91 }
92
93 /** Perform correction step.
94 * @param measurement single measurement to handle
95 * @param stm state transition matrix
96 * @param innovation innovation vector (i.e. residuals)
97 * (may be null if measurement should be ignored)
98 * @param h Jacobian of the measurement with respect to the state
99 * (may be null if measurement should be ignored)
100 * @param s innovation covariance matrix
101 * (may be null if measurement should be ignored)
102 * @exception MathIllegalArgumentException if matrix cannot be decomposed
103 */
104 protected void correct(final T measurement, final RealMatrix stm, final RealVector innovation,
105 final RealMatrix h, final RealMatrix s)
106 throws MathIllegalArgumentException {
107
108 if (innovation == null) {
109 // measurement should be ignored
110 corrected = predicted;
111 return;
112 }
113
114 // compute Kalman gain k
115 // the following is equivalent to k = p.h^T * (h.p.h^T + r)^(-1)
116 // we don't want to compute the inverse of a matrix,
117 // we start by post-multiplying by h.p.h^T + r and get
118 // k.(h.p.h^T + r) = p.h^T
119 // then we transpose, knowing that both p and r are symmetric matrices
120 // (h.p.h^T + r).k^T = h.p
121 // then we can use linear system solving instead of matrix inversion
122 final RealMatrix k = decomposer.
123 decompose(s).
124 solve(h.multiply(predicted.getCovariance())).
125 transpose();
126
127 // correct state vector
128 final RealVector correctedState = predicted.getState().add(k.operate(innovation));
129
130 // here we use the Joseph algorithm (see "Fundamentals of Astrodynamics and Applications,
131 // Vallado, Fourth Edition §10.6 eq.10-34) which is equivalent to
132 // the traditional Pest = (I - k.h) x Ppred expression but guarantees the output stays symmetric:
133 // Pest = (I -k.h) Ppred (I - k.h)^T + k.r.k^T
134 final RealMatrix idMkh = k.multiply(h);
135 for (int i = 0; i < idMkh.getRowDimension(); ++i) {
136 for (int j = 0; j < idMkh.getColumnDimension(); ++j) {
137 idMkh.multiplyEntry(i, j, -1);
138 }
139 idMkh.addToEntry(i, i, 1.0);
140 }
141 final RealMatrix r = measurement.getCovariance();
142 final RealMatrix correctedCovariance =
143 idMkh.multiply(predicted.getCovariance()).multiplyTransposed(idMkh).
144 add(k.multiply(r).multiplyTransposed(k));
145
146 corrected = new ProcessEstimate(measurement.getTime(), correctedState, correctedCovariance,
147 stm, h, s, k);
148
149 }
150
151 /** Get the observer.
152 * @return the observer
153 */
154 protected KalmanObserver getObserver() {
155 return observer;
156 }
157
158
159 /** {@inheritDoc} */
160 @Override
161 public void setObserver(final KalmanObserver kalmanObserver) {
162 observer = kalmanObserver;
163 observer.init(this);
164 }
165
166 /** Get the predicted state.
167 * @return predicted state
168 */
169 @Override
170 public ProcessEstimate getPredicted() {
171 return predicted;
172 }
173
174 /** Get the corrected state.
175 * @return corrected state
176 */
177 @Override
178 public ProcessEstimate getCorrected() {
179 return corrected;
180 }
181
182 /** {@inheritDoc} */
183 @Override
184 public RealMatrix getStateCrossCovariance() {
185 return priorCovariance.multiplyTransposed(stateTransitionMatrix);
186 }
187 }