1 | |
---|
2 | #include "kalman.h" |
---|
3 | |
---|
4 | namespace bdm{ |
---|
5 | |
---|
6 | using std::endl; |
---|
7 | |
---|
8 | KalmanFull::KalmanFull ( mat A0, mat B0, mat C0, mat D0, mat Q0, mat R0, mat P0, vec mu0 ) { |
---|
9 | dimx = A0.rows(); |
---|
10 | dimu = B0.cols(); |
---|
11 | dimy = C0.rows(); |
---|
12 | |
---|
13 | it_assert_debug ( A0.cols() ==dimx, "KalmanFull: A is not square" ); |
---|
14 | it_assert_debug ( B0.rows() ==dimx, "KalmanFull: B is not compatible" ); |
---|
15 | it_assert_debug ( C0.cols() ==dimx, "KalmanFull: C is not square" ); |
---|
16 | it_assert_debug ( ( D0.rows() ==dimy ) || ( D0.cols() ==dimu ), "KalmanFull: D is not compatible" ); |
---|
17 | it_assert_debug ( ( Q0.cols() ==dimx ) || ( Q0.rows() ==dimx ), "KalmanFull: Q is not compatible" ); |
---|
18 | it_assert_debug ( ( R0.cols() ==dimy ) || ( R0.rows() ==dimy ), "KalmanFull: R is not compatible" ); |
---|
19 | |
---|
20 | A = A0; |
---|
21 | B = B0; |
---|
22 | C = C0; |
---|
23 | D = D0; |
---|
24 | R = R0; |
---|
25 | Q = Q0; |
---|
26 | mu = mu0; |
---|
27 | P = P0; |
---|
28 | |
---|
29 | ll = 0.0; |
---|
30 | evalll=true; |
---|
31 | } |
---|
32 | |
---|
33 | void KalmanFull::bayes ( const vec &dt ) { |
---|
34 | it_assert_debug ( dt.length() == ( dimy+dimu ),"KalmanFull::bayes wrong size of dt" ); |
---|
35 | |
---|
36 | vec u = dt.get ( dimy,dimy+dimu-1 ); |
---|
37 | vec y = dt.get ( 0,dimy-1 ); |
---|
38 | //Time update |
---|
39 | mu = A*mu + B*u; |
---|
40 | P = A*P*A.transpose() + Q; |
---|
41 | |
---|
42 | //Data update |
---|
43 | _Ry = C*P*C.transpose() + R; |
---|
44 | _iRy = inv ( _Ry ); |
---|
45 | _K = P*C.transpose() *_iRy; |
---|
46 | P -= _K*C*P; // P = P -KCP; |
---|
47 | mu += _K* ( y-C*mu-D*u ); |
---|
48 | |
---|
49 | if ( evalll ) { |
---|
50 | //from enorm |
---|
51 | vec x=y-C*mu-D*u; |
---|
52 | ll = -0.5* ( R.cols() * 1.83787706640935 +log ( det ( _Ry ) ) +x* ( _iRy*x ) ); |
---|
53 | } |
---|
54 | }; |
---|
55 | |
---|
56 | std::ostream &operator<< ( std::ostream &os, const KalmanFull &kf ) { |
---|
57 | os << "mu:" << kf.mu << endl << "P:" << kf.P <<endl; |
---|
58 | return os; |
---|
59 | } |
---|
60 | |
---|
61 | |
---|
62 | |
---|
63 | /////////////////////////////// EKFS |
---|
64 | EKFfull::EKFfull ( ) : BM (),E() {}; |
---|
65 | |
---|
66 | void EKFfull::set_parameters ( diffbifn* pfxu0, diffbifn* phxu0,const mat Q0,const mat R0 ) { |
---|
67 | pfxu = pfxu0; |
---|
68 | phxu = phxu0; |
---|
69 | |
---|
70 | dimx = pfxu0->_dimx(); |
---|
71 | dimy = phxu0->dimension(); |
---|
72 | dimu = pfxu0->_dimu(); |
---|
73 | E.epdf::set_parameters(dimx); |
---|
74 | |
---|
75 | A.set_size(dimx,dimx); |
---|
76 | C.set_size(dimy,dimx); |
---|
77 | //initialize matrices A C, later, these will be only updated! |
---|
78 | pfxu->dfdx_cond ( mu,zeros ( dimu ),A,true ); |
---|
79 | B.clear(); |
---|
80 | phxu->dfdx_cond ( mu,zeros ( dimu ),C,true ); |
---|
81 | D.clear(); |
---|
82 | |
---|
83 | R = R0; |
---|
84 | Q = Q0; |
---|
85 | } |
---|
86 | |
---|
87 | void EKFfull::bayes ( const vec &dt ) { |
---|
88 | it_assert_debug ( dt.length() == ( dimy+dimu ),"KalmanFull::bayes wrong size of dt" ); |
---|
89 | |
---|
90 | vec u = dt.get ( dimy,dimy+dimu-1 ); |
---|
91 | vec y = dt.get ( 0,dimy-1 ); |
---|
92 | |
---|
93 | pfxu->dfdx_cond ( mu,zeros ( dimu ),A,true ); |
---|
94 | phxu->dfdx_cond ( mu,zeros ( dimu ),C,true ); |
---|
95 | |
---|
96 | //Time update |
---|
97 | mu = pfxu->eval(mu,u);// A*mu + B*u; |
---|
98 | P = A*P*A.transpose() + Q; |
---|
99 | |
---|
100 | //Data update |
---|
101 | _Ry = C*P*C.transpose() + R; |
---|
102 | _iRy = inv ( _Ry ); |
---|
103 | _K = P*C.transpose() *_iRy; |
---|
104 | P -= _K*C*P; // P = P -KCP; |
---|
105 | vec yp = phxu->eval(mu,u); |
---|
106 | mu += _K* ( y-yp ); |
---|
107 | |
---|
108 | E.set_mu(mu); |
---|
109 | |
---|
110 | if ( BM::evalll ) { |
---|
111 | //from enorm |
---|
112 | vec x=y-yp; |
---|
113 | BM::ll = -0.5* ( R.cols() * 1.83787706640935 +log ( det ( _Ry ) ) +x* ( _iRy*x ) ); |
---|
114 | } |
---|
115 | }; |
---|
116 | |
---|
117 | |
---|
118 | |
---|
119 | void KalmanCh::set_parameters ( const mat &A0,const mat &B0,const mat &C0,const mat &D0,const chmat &Q0,const chmat &R0 ) { |
---|
120 | |
---|
121 | ( ( Kalman<chmat>* ) this )->set_parameters ( A0,B0,C0,D0,Q0,R0 ); |
---|
122 | // Cholesky special! |
---|
123 | preA=zeros(dimy+dimx+dimx,dimy+dimx); |
---|
124 | // preA.clear(); |
---|
125 | preA.set_submatrix ( 0,0,R._Ch() ); |
---|
126 | preA.set_submatrix ( dimy+dimx,dimy,Q._Ch() ); |
---|
127 | } |
---|
128 | |
---|
129 | |
---|
130 | void KalmanCh::bayes ( const vec &dt ) { |
---|
131 | |
---|
132 | vec u = dt.get ( dimy,dimy+dimu-1 ); |
---|
133 | vec y = dt.get ( 0,dimy-1 ); |
---|
134 | vec pom(dimy); |
---|
135 | |
---|
136 | //TODO get rid of Q in qr()! |
---|
137 | // mat Q; |
---|
138 | |
---|
139 | //R and Q are already set in set_parameters() |
---|
140 | preA.set_submatrix ( dimy,0, ( _P._Ch() ) *C.T() ); |
---|
141 | //Fixme can be more efficient if .T() is not used |
---|
142 | preA.set_submatrix ( dimy,dimy, ( _P._Ch() ) *A.T() ); |
---|
143 | |
---|
144 | // if ( !qr ( preA,Q,postA ) ) it_warning ( "QR in kalman unstable!" ); |
---|
145 | if ( !qr ( preA,postA ) ) {it_warning ( "QR in kalman unstable!" );} |
---|
146 | |
---|
147 | ( _Ry._Ch() ) =postA ( 0,dimy-1, 0,dimy-1 ); |
---|
148 | _K = inv(A)*( postA ( 0,dimy-1 ,dimy,dimy+dimx-1 ) ).T(); |
---|
149 | ( _P._Ch() ) = postA ( dimy,dimy+dimx-1,dimy,dimy+dimx-1 ); |
---|
150 | |
---|
151 | _mu = A* ( _mu ) + B*u; |
---|
152 | _yp = C*_mu -D*u; |
---|
153 | |
---|
154 | backward_substitution(_Ry._Ch(),( y-_yp),pom); |
---|
155 | _mu += ( _K ) * pom; |
---|
156 | |
---|
157 | /* cout << "P:" <<_P.to_mat() <<endl; |
---|
158 | cout << "Ry:" <<_Ry.to_mat() <<endl; |
---|
159 | cout << "_K:" <<_K <<endl;*/ |
---|
160 | |
---|
161 | if ( evalll==true ) { //likelihood of observation y |
---|
162 | ll=fy.evallog ( y ); |
---|
163 | } |
---|
164 | } |
---|
165 | |
---|
166 | |
---|
167 | |
---|
168 | void EKFCh::set_parameters ( diffbifn* pfxu0, diffbifn* phxu0,const chmat Q0,const chmat R0 ) { |
---|
169 | pfxu = pfxu0; |
---|
170 | phxu = phxu0; |
---|
171 | |
---|
172 | dimx = pfxu0->dimension(); |
---|
173 | dimy = phxu0->dimension(); |
---|
174 | dimu = pfxu0->_dimu(); |
---|
175 | // if mu is not set, set it to zeros, just for constant terms of A and C |
---|
176 | if (_mu.length()!=dimx) _mu=zeros(dimx); |
---|
177 | A=zeros(dimx,dimx); |
---|
178 | C=zeros(dimy,dimx); |
---|
179 | preA=zeros(dimy+dimx+dimx, dimy+dimx); |
---|
180 | |
---|
181 | //initialize matrices A C, later, these will be only updated! |
---|
182 | pfxu->dfdx_cond ( _mu,zeros ( dimu ),A,true ); |
---|
183 | // pfxu->dfdu_cond ( *_mu,zeros ( dimu ),B,true ); |
---|
184 | B.clear(); |
---|
185 | phxu->dfdx_cond ( _mu,zeros ( dimu ),C,true ); |
---|
186 | // phxu->dfdu_cond ( *_mu,zeros ( dimu ),D,true ); |
---|
187 | D.clear(); |
---|
188 | |
---|
189 | R = R0; |
---|
190 | Q = Q0; |
---|
191 | |
---|
192 | // Cholesky special! |
---|
193 | preA.clear(); |
---|
194 | preA.set_submatrix ( 0,0,R._Ch() ); |
---|
195 | preA.set_submatrix ( dimy+dimx,dimy,Q._Ch() ); |
---|
196 | } |
---|
197 | |
---|
198 | |
---|
199 | void EKFCh::bayes ( const vec &dt ) { |
---|
200 | |
---|
201 | vec pom(dimy); |
---|
202 | vec u = dt.get ( dimy,dimy+dimu-1 ); |
---|
203 | vec y = dt.get ( 0,dimy-1 ); |
---|
204 | |
---|
205 | pfxu->dfdx_cond ( _mu,u,A,false ); //update A by a derivative of fx |
---|
206 | phxu->dfdx_cond ( _mu,u,C,false ); //update A by a derivative of fx |
---|
207 | |
---|
208 | //R and Q are already set in set_parameters() |
---|
209 | preA.set_submatrix ( dimy,0, ( _P._Ch() ) *C.T() ); |
---|
210 | //Fixme can be more efficient if .T() is not used |
---|
211 | preA.set_submatrix ( dimy,dimy, ( _P._Ch() ) *A.T() ); |
---|
212 | |
---|
213 | // mat Sttm = _P->to_mat(); |
---|
214 | // cout << preA <<endl; |
---|
215 | // cout << "_mu:" << _mu <<endl; |
---|
216 | |
---|
217 | // if ( !qr ( preA,Q,postA ) ) it_warning ( "QR in kalman unstable!" ); |
---|
218 | if ( !qr ( preA,postA ) ) {it_warning ( "QR in kalman unstable!" );} |
---|
219 | |
---|
220 | |
---|
221 | ( _Ry._Ch() ) =postA ( 0,dimy-1, 0,dimy-1 ); |
---|
222 | _K = inv(A)*( postA ( 0,dimy-1 ,dimy,dimy+dimx-1 ) ).T(); |
---|
223 | ( _P._Ch() ) = postA ( dimy,dimy+dimx-1,dimy,dimy+dimx-1 ); |
---|
224 | |
---|
225 | // mat iRY = inv(_Ry->to_mat()); |
---|
226 | // mat Stt = Sttm - Sttm * C.T() * iRY * C * Sttm; |
---|
227 | // mat _K2 = Stt*C.T()*inv(R.to_mat()); |
---|
228 | |
---|
229 | // prediction |
---|
230 | _mu = pfxu->eval ( _mu ,u ); |
---|
231 | _yp = phxu->eval ( _mu,u ); |
---|
232 | |
---|
233 | /* vec mu2 = *_mu + ( _K2 ) * ( y-*_yp );*/ |
---|
234 | |
---|
235 | //correction //= initial value is already prediction! |
---|
236 | backward_substitution(_Ry._Ch(),( y-_yp ),pom); |
---|
237 | _mu += ( _K ) *pom ; |
---|
238 | |
---|
239 | /* _K = (_P->to_mat())*C.transpose() * ( _iRy->to_mat() ); |
---|
240 | *_mu = pfxu->eval ( *_mu ,u ) + ( _K )* ( y-*_yp );*/ |
---|
241 | |
---|
242 | // cout << "P:" <<_P.to_mat() <<endl; |
---|
243 | // cout << "Ry:" <<_Ry.to_mat() <<endl; |
---|
244 | // cout << "_mu:" <<_mu <<endl; |
---|
245 | // cout << "dt:" <<dt <<endl; |
---|
246 | |
---|
247 | if ( evalll==true ) { //likelihood of observation y |
---|
248 | ll=fy.evallog ( y ); |
---|
249 | } |
---|
250 | } |
---|
251 | |
---|
252 | void EKFCh::from_setting( const Setting &set ) |
---|
253 | { |
---|
254 | diffbifn* IM = UI::build<diffbifn>(set, "IM", UI::compulsory); |
---|
255 | diffbifn* OM = UI::build<diffbifn>(set, "OM", UI::compulsory); |
---|
256 | |
---|
257 | //statistics |
---|
258 | int dim=IM->dimension(); |
---|
259 | vec mu0; |
---|
260 | if(!UI::get( mu0, set, "mu0")) |
---|
261 | mu0=zeros(dim); |
---|
262 | |
---|
263 | mat P0; |
---|
264 | vec dP0; |
---|
265 | if(UI::get( dP0, set, "dP0")) |
---|
266 | P0=diag(dP0); |
---|
267 | else if ( !UI::get(P0, set, "P0") ) |
---|
268 | P0=eye(dim); |
---|
269 | |
---|
270 | set_statistics(mu0,P0); |
---|
271 | |
---|
272 | //parameters |
---|
273 | vec dQ, dR; |
---|
274 | UI::get( dQ, set, "dQ", UI::compulsory); |
---|
275 | UI::get( dR, set, "dR", UI::compulsory); |
---|
276 | set_parameters(IM, OM, diag(dQ), diag(dR)); |
---|
277 | |
---|
278 | //connect |
---|
279 | RV* drv = UI::build<RV>(set, "drv", UI::compulsory); |
---|
280 | set_drv(*drv); |
---|
281 | RV* rv = UI::build<RV>(set, "rv", UI::compulsory); |
---|
282 | set_rv(*rv); |
---|
283 | |
---|
284 | string options; |
---|
285 | if( UI::get( options, set, "options" ) ) |
---|
286 | set_options(options); |
---|
287 | } |
---|
288 | |
---|
289 | void MultiModel::from_setting( const Setting &set ) |
---|
290 | { |
---|
291 | Array<EKFCh*> A; |
---|
292 | UI::get( A, set, "models", UI::compulsory); |
---|
293 | |
---|
294 | set_parameters(A); |
---|
295 | set_drv(A(0)->_drv()); |
---|
296 | //set_rv(A(0)->_rv()); |
---|
297 | |
---|
298 | string options; |
---|
299 | if(set.lookupValue( "options", options )) |
---|
300 | set_options(options); |
---|
301 | } |
---|
302 | |
---|
303 | } |
---|