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;
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 }