root/library/bdm/estim/kalman.cpp @ 384

Revision 384, 8.0 kB (checked in by mido, 15 years ago)

possibly broken?

  • Property svn:eol-style set to native
Line 
1
2#include "kalman.h"
3
4namespace bdm{
5
6using std::endl;
7
8KalmanFull::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
33void 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
56std::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
64EKFfull::EKFfull ( ) : BM (),E() {};
65
66void 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
87void 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
119void 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
130void 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
168void 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
199void 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
252void EKFCh::from_setting( const Setting &set ) 
253{       
254        diffbifn* IM = UI::build<diffbifn>(set, "IM");
255        diffbifn* OM = UI::build<diffbifn>(set, "OM");
256       
257        //statistics
258        int dim=IM->dimension();
259        vec mu0;
260        if(set.exists("mu0"))
261                UI::get( mu0, set, "mu0");
262        else
263                mu0=zeros(dim);
264
265        mat P0;
266        if(set.exists("dP0"))
267        {
268                vec dP0;
269                UI::get( dP0, set, "dP0");
270                P0=diag(dP0);
271        }
272        else if ( set.exists( "P0" ) )
273                UI::get(P0, set, "P0");
274        else
275                P0=eye(dim);
276       
277        set_statistics(mu0,P0);
278       
279        //parameters
280        vec dQ, dR;
281        UI::get( dQ, set, "dQ");
282        UI::get( dR, set, "dR");
283        set_parameters(IM, OM, diag(dQ), diag(dR));
284
285        //connect
286        RV* drv = UI::build<RV>(set, "drv");
287        set_drv(*drv);
288        RV* rv = UI::build<RV>(set, "rv");
289        set_rv(*rv);
290       
291        string options;
292        if(set.lookupValue( "options", options ))
293                set_options(options);   
294}
295
296/*void EKFCh::to_setting( Setting &set ) const
297{       
298        Transport::to_setting( set );
299
300        Setting &kilometers_setting = set.add("kilometers", Setting::TypeInt );
301        kilometers_setting = kilometers;
302
303        UI::save( passengers, set, "passengers" );
304}*/
305
306void MultiModel::from_setting( const Setting &set ) 
307{       
308        Array<EKFCh*> A;
309        UI::get( A, set, "models");
310       
311        set_parameters(A);
312        set_drv(A(0)->_drv());
313        //set_rv(A(0)->_rv());
314
315        string options;
316        if(set.lookupValue( "options", options ))
317                set_options(options);   
318}
319
320/*void MultiModel::to_setting( Setting &set ) const
321{       
322        Transport::to_setting( set );
323
324        Setting &kilometers_setting = set.add("kilometers", Setting::TypeInt );
325        kilometers_setting = kilometers;
326
327        UI::save( passengers, set, "passengers" );
328}*/
329
330
331}
Note: See TracBrowser for help on using the browser.