Show
Ignore:
Timestamp:
08/05/09 14:40:03 (15 years ago)
Author:
mido
Message:

panove, vite, jak jsem peclivej na upravu kodu.. snad se vam bude libit:) konfigurace je v souboru /system/astylerc

Files:
1 modified

Legend:

Unmodified
Added
Removed

  • library/bdm/estim/kalman.cpp

    r471 r477  
    22#include "kalman.h" 
    33 
    4 namespace bdm{ 
     4namespace bdm { 
    55 
    66using std::endl; 
    … …  
    1111        dimy = C0.rows(); 
    1212 
    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" ); 
     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" ); 
    1919 
    2020        A = A0; 
    … …  
    2626        mu = mu0; 
    2727        P = P0; 
    28          
     28 
    2929        ll = 0.0; 
    30         evalll=true; 
     30        evalll = true; 
    3131} 
    3232 
    3333void 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 ); 
     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 ); 
    3838        //Time update 
    39         mu = A*mu + B*u; 
    40         P  = A*P*A.transpose() + Q; 
     39        mu = A * mu + B * u; 
     40        P  = A * P * A.transpose() + Q; 
    4141 
    4242        //Data update 
    43         _Ry = C*P*C.transpose() + R; 
     43        _Ry = C * P * C.transpose() + R; 
    4444        _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 ); 
     45        _K = P * C.transpose() * _iRy; 
     46        P -= _K * C * P; // P = P -KCP; 
     47        mu += _K * ( y - C * mu - D * u ); 
    4848 
    4949        if ( evalll ) { 
    5050                //from enorm 
    51                 vec x=y-C*mu-D*u; 
    52                 ll = -0.5* ( R.cols() * 1.83787706640935 +log ( det ( _Ry ) ) +x* ( _iRy*x ) ); 
     51                vec x = y - C * mu - D * u; 
     52                ll = -0.5 * ( R.cols() * 1.83787706640935 + log ( det ( _Ry ) ) + x * ( _iRy * x ) ); 
    5353        } 
    5454}; 
    5555 
    5656std::ostream &operator<< ( std::ostream &os, const KalmanFull &kf ) { 
    57         os << "mu:" << kf.mu << endl << "P:" << kf.P <<endl; 
     57        os << "mu:" << kf.mu << endl << "P:" << kf.P << endl; 
    5858        return os; 
    5959} 
    … …  
    6161 
    6262 
    63  /////////////////////////////// EKFS 
    64 EKFfull::EKFfull ( ) : BM (),E() {}; 
    65  
    66 void EKFfull::set_parameters ( diffbifn* pfxu0,  diffbifn* phxu0,const mat Q0,const mat R0 ) { 
     63/////////////////////////////// EKFS 
     64EKFfull::EKFfull ( ) : BM (), E() {}; 
     65 
     66void EKFfull::set_parameters ( diffbifn* pfxu0,  diffbifn* phxu0, const mat Q0, const mat R0 ) { 
    6767        pfxu = pfxu0; 
    6868        phxu = phxu0; 
    … …  
    7171        dimy = phxu0->dimension(); 
    7272        dimu = pfxu0->_dimu(); 
    73         E.epdf::set_parameters(dimx); 
    74          
    75         A.set_size(dimx,dimx); 
    76         C.set_size(dimy,dimx); 
     73        E.epdf::set_parameters ( dimx ); 
     74 
     75        A.set_size ( dimx, dimx ); 
     76        C.set_size ( dimy, dimx ); 
    7777        //initialize matrices A C, later, these will be only updated! 
    78         pfxu->dfdx_cond ( mu,zeros ( dimu ),A,true ); 
     78        pfxu->dfdx_cond ( mu, zeros ( dimu ), A, true ); 
    7979        B.clear(); 
    80         phxu->dfdx_cond ( mu,zeros ( dimu ),C,true ); 
     80        phxu->dfdx_cond ( mu, zeros ( dimu ), C, true ); 
    8181        D.clear(); 
    8282 
    … …  
    8686 
    8787void 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 ); 
     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 ); 
    9595 
    9696        //Time update 
    97         mu = pfxu->eval(mu,u);// A*mu + B*u; 
    98         P  = A*P*A.transpose() + Q; 
     97        mu = pfxu->eval ( mu, u );// A*mu + B*u; 
     98        P  = A * P * A.transpose() + Q; 
    9999 
    100100        //Data update 
    101         _Ry = C*P*C.transpose() + R; 
     101        _Ry = C * P * C.transpose() + R; 
    102102        _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); 
     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 ); 
    109109 
    110110        if ( BM::evalll ) { 
    111111                //from enorm 
    112                 vec x=y-yp; 
    113                 BM::ll = -0.5* ( R.cols() * 1.83787706640935 +log ( det ( _Ry ) ) +x* ( _iRy*x ) ); 
     112                vec x = y - yp; 
     113                BM::ll = -0.5 * ( R.cols() * 1.83787706640935 + log ( det ( _Ry ) ) + x * ( _iRy * x ) ); 
    114114        } 
    115115}; 
    … …  
    117117 
    118118 
    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 ); 
     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 ); 
    122122        // Cholesky special! 
    123         preA=zeros(dimy+dimx+dimx,dimy+dimx); 
     123        preA = zeros ( dimy + dimx + dimx, dimy + dimx ); 
    124124//      preA.clear(); 
    125         preA.set_submatrix ( 0,0,R._Ch() ); 
    126         preA.set_submatrix ( dimy+dimx,dimy,Q._Ch() ); 
     125        preA.set_submatrix ( 0, 0, R._Ch() ); 
     126        preA.set_submatrix ( dimy + dimx, dimy, Q._Ch() ); 
    127127} 
    128128 
    … …  
    130130void KalmanCh::bayes ( const vec &dt ) { 
    131131 
    132         vec u = dt.get ( dimy,dimy+dimu-1 ); 
    133         vec y = dt.get ( 0,dimy-1 ); 
    134         vec pom(dimy); 
    135          
     132        vec u = dt.get ( dimy, dimy + dimu - 1 ); 
     133        vec y = dt.get ( 0, dimy - 1 ); 
     134        vec pom ( dimy ); 
     135 
    136136        //TODO get rid of Q in qr()! 
    137137//      mat Q; 
    138138 
    139139        //R and Q are already set in set_parameters() 
    140         preA.set_submatrix ( dimy,0, ( _P._Ch() ) *C.T() ); 
     140        preA.set_submatrix ( dimy, 0, ( _P._Ch() ) *C.T() ); 
    141141        //Fixme can be more efficient if .T() is not used 
    142         preA.set_submatrix ( dimy,dimy, ( _P._Ch() ) *A.T() ); 
     142        preA.set_submatrix ( dimy, dimy, ( _P._Ch() ) *A.T() ); 
    143143 
    144144//      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 ) { 
     145        if ( !qr ( preA, postA ) ) { 
     146                it_warning ( "QR in kalman unstable!" ); 
     147        } 
     148 
     149        ( _Ry._Ch() ) = postA ( 0, dimy - 1, 0, dimy - 1 ); 
     150        _K = inv ( A ) * ( postA ( 0, dimy - 1 , dimy, dimy + dimx - 1 ) ).T(); 
     151        ( _P._Ch() ) = postA ( dimy, dimy + dimx - 1, dimy, dimy + dimx - 1 ); 
     152 
     153        _mu = A * ( _mu ) + B * u; 
     154        _yp = C * _mu - D * u; 
     155 
     156        backward_substitution ( _Ry._Ch(), ( y - _yp ), pom ); 
     157        _mu += ( _K ) * pom; 
     158 
     159        /*              cout << "P:" <<_P.to_mat() <<endl; 
     160                        cout << "Ry:" <<_Ry.to_mat() <<endl; 
     161                        cout << "_K:" <<_K <<endl;*/ 
     162 
     163        if ( evalll == true ) { //likelihood of observation y 
     164                ll = fy.evallog ( y ); 
     165        } 
     166} 
     167 
     168 
     169 
     170void EKFCh::set_parameters ( diffbifn* pfxu0,  diffbifn* phxu0, const chmat Q0, const chmat R0 ) { 
    169171        pfxu = pfxu0; 
    170172        phxu = phxu0; 
    … …  
    173175        dimy = phxu0->dimension(); 
    174176        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          
     177        // if mu is not set, set it to zeros, just for constant terms of A and C 
     178        if ( _mu.length() != dimx ) _mu = zeros ( dimx ); 
     179        A = zeros ( dimx, dimx ); 
     180        C = zeros ( dimy, dimx ); 
     181        preA = zeros ( dimy + dimx + dimx, dimy + dimx ); 
     182 
    181183        //initialize matrices A C, later, these will be only updated! 
    182         pfxu->dfdx_cond ( _mu,zeros ( dimu ),A,true ); 
     184        pfxu->dfdx_cond ( _mu, zeros ( dimu ), A, true ); 
    183185//      pfxu->dfdu_cond ( *_mu,zeros ( dimu ),B,true ); 
    184186        B.clear(); 
    185         phxu->dfdx_cond ( _mu,zeros ( dimu ),C,true ); 
     187        phxu->dfdx_cond ( _mu, zeros ( dimu ), C, true ); 
    186188//      phxu->dfdu_cond ( *_mu,zeros ( dimu ),D,true ); 
    187189        D.clear(); 
    … …  
    192194        // Cholesky special! 
    193195        preA.clear(); 
    194         preA.set_submatrix ( 0,0,R._Ch() ); 
    195         preA.set_submatrix ( dimy+dimx,dimy,Q._Ch() ); 
     196        preA.set_submatrix ( 0, 0, R._Ch() ); 
     197        preA.set_submatrix ( dimy + dimx, dimy, Q._Ch() ); 
    196198} 
    197199 
    … …  
    199201void EKFCh::bayes ( const vec &dt ) { 
    200202 
    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 
     203        vec pom ( dimy ); 
     204        vec u = dt.get ( dimy, dimy + dimu - 1 ); 
     205        vec y = dt.get ( 0, dimy - 1 ); 
     206 
     207        pfxu->dfdx_cond ( _mu, u, A, false ); //update A by a derivative of fx 
     208        phxu->dfdx_cond ( _mu, u, C, false ); //update A by a derivative of fx 
    207209 
    208210        //R and Q are already set in set_parameters() 
    209         preA.set_submatrix ( dimy,0, ( _P._Ch() ) *C.T() ); 
     211        preA.set_submatrix ( dimy, 0, ( _P._Ch() ) *C.T() ); 
    210212        //Fixme can be more efficient if .T() is not used 
    211         preA.set_submatrix ( dimy,dimy, ( _P._Ch() ) *A.T() ); 
     213        preA.set_submatrix ( dimy, dimy, ( _P._Ch() ) *A.T() ); 
    212214 
    213215//      mat Sttm = _P->to_mat(); 
    … …  
    216218 
    217219//      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          
     220        if ( !qr ( preA, postA ) ) { 
     221                it_warning ( "QR in kalman unstable!" ); 
     222        } 
     223 
     224 
     225        ( _Ry._Ch() ) = postA ( 0, dimy - 1, 0, dimy - 1 ); 
     226        _K = inv ( A ) * ( postA ( 0, dimy - 1 , dimy, dimy + dimx - 1 ) ).T(); 
     227        ( _P._Ch() ) = postA ( dimy, dimy + dimx - 1, dimy, dimy + dimx - 1 ); 
     228 
    225229//      mat iRY = inv(_Ry->to_mat()); 
    226230//      mat Stt = Sttm - Sttm * C.T() * iRY * C * Sttm; 
    … …  
    228232 
    229233        // prediction 
    230         _mu = pfxu->eval ( _mu ,u ); 
    231         _yp = phxu->eval ( _mu,u ); 
    232          
    233 /*      vec mu2 = *_mu + ( _K2 ) * ( y-*_yp );*/ 
    234          
     234        _mu = pfxu->eval ( _mu , u ); 
     235        _yp = phxu->eval ( _mu, u ); 
     236 
     237        /*      vec mu2 = *_mu + ( _K2 ) * ( y-*_yp );*/ 
     238 
    235239        //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          
     240        backward_substitution ( _Ry._Ch(), ( y - _yp ), pom ); 
     241        _mu += ( _K ) * pom ; 
     242 
     243        /*      _K = (_P->to_mat())*C.transpose() * ( _iRy->to_mat() ); 
     244                *_mu = pfxu->eval ( *_mu ,u ) + ( _K )* ( y-*_yp );*/ 
     245 
    242246//              cout << "P:" <<_P.to_mat() <<endl; 
    243247//              cout << "Ry:" <<_Ry.to_mat() <<endl; 
    … …  
    245249//      cout << "dt:" <<dt <<endl; 
    246250 
    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          
     251        if ( evalll == true ) { //likelihood of observation y 
     252                ll = fy.evallog ( y ); 
     253        } 
     254} 
     255 
     256void EKFCh::from_setting ( const Setting &set ) { 
     257        diffbifn* IM = UI::build<diffbifn> ( set, "IM", UI::compulsory ); 
     258        diffbifn* OM = UI::build<diffbifn> ( set, "OM", UI::compulsory ); 
     259 
    257260        //statistics 
    258         int dim=IM->dimension(); 
     261        int dim = IM->dimension(); 
    259262        vec mu0; 
    260         if(!UI::get( mu0, set, "mu0")) 
    261                 mu0=zeros(dim); 
     263        if ( !UI::get ( mu0, set, "mu0" ) ) 
     264                mu0 = zeros ( dim ); 
    262265 
    263266        mat P0; 
    264267        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          
     268        if ( UI::get ( dP0, set, "dP0" ) ) 
     269                P0 = diag ( dP0 ); 
     270        else if ( !UI::get ( P0, set, "P0" ) ) 
     271                P0 = eye ( dim ); 
     272 
     273        set_statistics ( mu0, P0 ); 
     274 
    272275        //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)); 
     276        vec dQ, dR; 
     277        UI::get ( dQ, set, "dQ", UI::compulsory ); 
     278        UI::get ( dR, set, "dR", UI::compulsory ); 
     279        set_parameters ( IM, OM, diag ( dQ ), diag ( dR ) ); 
    277280 
    278281        //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          
     282        RV* drv = UI::build<RV> ( set, "drv", UI::compulsory ); 
     283        set_drv ( *drv ); 
     284        RV* rv = UI::build<RV> ( set, "rv", UI::compulsory ); 
     285        set_rv ( *rv ); 
     286 
    284287        string options; 
    285         if( UI::get( options, set, "options" ) ) 
    286                 set_options(options);    
    287 } 
    288  
    289 void MultiModel::from_setting( const Setting &set )  
    290 {        
     288        if ( UI::get ( options, set, "options" ) ) 
     289                set_options ( options ); 
     290} 
     291 
     292void MultiModel::from_setting ( const Setting &set ) { 
    291293        Array<EKFCh*> A; 
    292         UI::get( A, set, "models", UI::compulsory); 
    293          
    294         set_parameters(A); 
    295         set_drv(A(0)->_drv()); 
     294        UI::get ( A, set, "models", UI::compulsory ); 
     295 
     296        set_parameters ( A ); 
     297        set_drv ( A ( 0 )->_drv() ); 
    296298        //set_rv(A(0)->_rv()); 
    297299 
    298300        string options; 
    299         if(set.lookupValue( "options", options )) 
    300                 set_options(options);    
    301 } 
    302  
    303 } 
     301        if ( set.lookupValue ( "options", options ) ) 
     302                set_options ( options ); 
     303} 
     304 
     305}