Edinburgh Speech Tools 2.4-release
EST_kalman.cc
1/*************************************************************************/
2/* */
3/* Centre for Speech Technology Research */
4/* University of Edinburgh, UK */
5/* Copyright (c) 1995,1996 */
6/* All Rights Reserved. */
7/* */
8/* Permission is hereby granted, free of charge, to use and distribute */
9/* this software and its documentation without restriction, including */
10/* without limitation the rights to use, copy, modify, merge, publish, */
11/* distribute, sublicense, and/or sell copies of this work, and to */
12/* permit persons to whom this work is furnished to do so, subject to */
13/* the following conditions: */
14/* 1. The code must retain the above copyright notice, this list of */
15/* conditions and the following disclaimer. */
16/* 2. Any modifications must be clearly marked as such. */
17/* 3. Original authors' names are not deleted. */
18/* 4. The authors' names are not used to endorse or promote products */
19/* derived from this software without specific prior written */
20/* permission. */
21/* */
22/* THE UNIVERSITY OF EDINBURGH AND THE CONTRIBUTORS TO THIS WORK */
23/* DISCLAIM ALL WARRANTIES WITH REGARD TO THIS SOFTWARE, INCLUDING */
24/* ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS, IN NO EVENT */
25/* SHALL THE UNIVERSITY OF EDINBURGH NOR THE CONTRIBUTORS BE LIABLE */
26/* FOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES */
27/* WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN */
28/* AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, */
29/* ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF */
30/* THIS SOFTWARE. */
31/* */
32/*************************************************************************/
33/* Author : Simon King */
34/* Date : June 1998 */
35/*-----------------------------------------------------------------------*/
36/* Kalman filtering, i.e. linear model fitting */
37/* */
38/*=======================================================================*/
39
40#include <cstdlib>
41#include <cstdio>
42#include <fstream>
43#include "EST.h"
44#include "EST_kalman.h"
45
46static bool kalman_filter_param_check(EST_FVector &x,
47 EST_FMatrix &P,
48 EST_FMatrix &Q,
49 EST_FMatrix &R,
50 EST_FMatrix &A,
51 EST_FMatrix &H,
52 EST_FVector &z);
53
54
55
56bool kalman_filter(EST_FVector &x,
57 EST_FMatrix &P,
58 EST_FMatrix &Q,
59 EST_FMatrix &R,
60 EST_FMatrix &A,
61 EST_FMatrix &H,
62 EST_FVector &z)
63{
64 // see kalman.h for meaning of args
65
66 if(!kalman_filter_param_check(x,P,Q,R,A,H,z))
67 {
68 cerr << "Kalman filter parameters inconsistent !" << endl;
69 return FALSE;
70 }
71
72 EST_FMatrix K,I,At,Ht,PHt,HPHt_R,HPHt_R_inv;
73 int state_dim=x.length();
74 int singularity;
75 eye(I,state_dim);
76 transpose(A,At);
77
78 cerr << "predict" << endl;
79
80 // predict
81 // =======
82 // if A is the identity matrix we could speed this up a LOT
83 x = A * x;
84 P = A * P * At + Q;
85
86 cerr << "correct" << endl;
87
88
89 // correct
90 // =======
91 // T T -1
92 // K = P H ( H P H + R )
93 transpose(H,Ht);
94 PHt = P * Ht;
95 HPHt_R=(H * PHt) + R;
96
97 if(!inverse( HPHt_R , HPHt_R_inv, singularity))
98 {
99 if(singularity != -1)
100 {
101 cerr << " H * P * Ht + R is singular !" << endl;
102 return FALSE;
103 }
104 cerr << "Matrix inversion failed for an unknown reason !" << endl;
105 return FALSE;
106 }
107
108 K = PHt * HPHt_R_inv;
109 x = add(x, K * subtract(z,H * x));
110 P = (I - K * H) * P;
111
112 // try and remedy numerical errors
113 symmetrize(P);
114
115 //cerr << "done" << endl;
116
117 return TRUE;
118}
119
120
121
122bool kalman_filter_Pinv(EST_FVector &x,
123 EST_FMatrix &Pinv,
124 EST_FMatrix &Q,
125 EST_FMatrix &Rinv,
126 EST_FMatrix &A,
127 EST_FMatrix &H,
128 EST_FVector &z)
129{
130
131 // a different formulation, using the inverse
132 // covariance matrix, and a more stable update
133 // equation
134
135 // from:
136 // Intro. to Random Signals and Kalman Applied Filtering
137 // Brown & Hwang (Wiley,1997)
138 // p. 248
139
140 if(!kalman_filter_param_check(x,Pinv,Q,Rinv,A,H,z))
141 {
142 cerr << "Kalman filter parameters inconsistent !" << endl;
143 return FALSE;
144 }
145
146
147 EST_FMatrix K,I,At,Ht,P;
148 int singularity;
149 int state_dim=x.length();
150 eye(I,state_dim);
151 transpose(A,At);
152 transpose(H,Ht);
153
154 cerr << "Compute P" << endl;
155
156
157 // update error covariance
158 // =======================
159 Pinv = Pinv + (Ht * Rinv * H);
160
161 if(!inverse(Pinv,P,singularity))
162 {
163 if(singularity != -1)
164 {
165 cerr << "P is singular !" << endl;
166 return FALSE;
167 }
168 cerr << "Matrix inversion failed for an unknown reason !" << endl;
169 return FALSE;
170 }
171
172 // compute gain
173 // ============
174 K = P * Ht * Rinv;
175
176 // update state
177 // ============
178 x = add(x, K * subtract(z,H*x));
179
180 // project ahead
181 // =============
182 x = A * x;
183 P = A * P * At + Q;
184 if(!inverse(P,Pinv,singularity))
185 {
186 if(singularity != -1)
187 {
188 cerr << "Pinv is singular !" << endl;
189 return FALSE;
190 }
191 cerr << "Matrix inversion failed for an unknown reason !" << endl;
192 return FALSE;
193 }
194
195 // try and remedy numerical errors
196 //symmetrize(P);
197
198 //cerr << "done" << endl;
199
200 return TRUE;
201
202}
203
204
205
206bool kalman_filter_param_check(EST_FVector &x,
207 EST_FMatrix &P,
208 EST_FMatrix &Q,
209 EST_FMatrix &R,
210 EST_FMatrix &A,
211 EST_FMatrix &H,
212 EST_FVector &z)
213{
214
215
216 int state_dim=x.length();
217 int measurement_dim=z.length();
218
219
220 // sanity checks
221 if((state_dim <= 0) ||
222 (measurement_dim <= 0))
223 {
224 cerr << "No state or measurements !!" << endl;
225 return FALSE;
226 }
227
228 // dimensionality
229
230 // P is error covariance
231 if((P.num_rows() != state_dim) ||
232 (P.num_columns() != state_dim) )
233 {
234 cerr << "P, or Pinv, must be a symmetrical square matrix of the same dimension" << endl;
235 cerr << "as the state vector, x" << endl;
236 return FALSE;
237 }
238
239 // Q is process noise covariance
240 if((Q.num_rows() != state_dim) ||
241 (Q.num_columns() != state_dim) )
242 {
243 cerr << "Q must be a symmetrical square matrix of the same dimension" << endl;
244 cerr << "as the state vector, x" << endl;
245 return FALSE;
246 }
247
248 // R is measurement noise covariance
249 if((R.num_rows() != measurement_dim) ||
250 (R.num_columns() != measurement_dim) )
251 {
252 cerr << "R, or Rinv, must be a symmetrical square matrix of the same dimension" << endl;
253 cerr << "as the measurement vector, z" << endl;
254 return FALSE;
255 }
256
257 if((A.num_rows() != state_dim) ||
258 (A.num_columns() != state_dim) )
259 {
260 cerr << "A must be a square matrix of the same dimension" << endl;
261 cerr << "as the state vector, x" << endl;
262 return FALSE;
263 }
264
265 if((H.num_rows() != measurement_dim) ||
266 (H.num_columns() != state_dim) )
267 {
268 cerr << "H must have dimensions to fit z = Hx" << endl;
269 return FALSE;
270 }
271
272 return TRUE;
273}
int num_columns() const
return number of columns
Definition: EST_TMatrix.h:181
int num_rows() const
return number of rows
Definition: EST_TMatrix.h:179
INLINE int length() const
number of items in vector.
Definition: EST_TVector.h:252