1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
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
27
28
29
30 public abstract class AbstractKalmanFilter<T extends Measurement> implements KalmanFilter<T> {
31
32
33 private final MatrixDecomposer decomposer;
34
35
36 private ProcessEstimate predicted;
37
38
39 private ProcessEstimate corrected;
40
41
42 private RealMatrix priorCovariance;
43
44
45 private RealMatrix stateTransitionMatrix;
46
47
48 private KalmanObserver observer;
49
50
51
52
53
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
64
65
66
67
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
79
80
81
82
83
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
94
95
96
97
98
99
100
101
102
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
110 corrected = predicted;
111 return;
112 }
113
114
115
116
117
118
119
120
121
122 final RealMatrix k = decomposer.
123 decompose(s).
124 solve(h.multiply(predicted.getCovariance())).
125 transpose();
126
127
128 final RealVector correctedState = predicted.getState().add(k.operate(innovation));
129
130
131
132
133
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
152
153
154 protected KalmanObserver getObserver() {
155 return observer;
156 }
157
158
159
160 @Override
161 public void setObserver(final KalmanObserver kalmanObserver) {
162 observer = kalmanObserver;
163 observer.init(this);
164 }
165
166
167
168
169 @Override
170 public ProcessEstimate getPredicted() {
171 return predicted;
172 }
173
174
175
176
177 @Override
178 public ProcessEstimate getCorrected() {
179 return corrected;
180 }
181
182
183 @Override
184 public RealMatrix getStateCrossCovariance() {
185 return priorCovariance.multiplyTransposed(stateTransitionMatrix);
186 }
187 }