Show
Ignore:
Timestamp:
06/09/10 14:00:40 (14 years ago)
Author:
mido
Message:

astyle applied all over the library

Files:
1 modified

Legend:

Unmodified
Added
Removed

  • library/bdm/estim/kalman.cpp

    r961 r1064  
    99 
    1010void KalmanFull::bayes ( const vec &yt, const vec &cond ) { 
    11         bdm_assert_debug ( yt.length() == ( dimy ), "KalmanFull::bayes wrong size of dt, " + 
    12                 num2str(yt.length()) + ", expected size is " + num2str(dimy) ); 
    13         bdm_assert_debug ( cond.length() == ( dimc ), "KalmanFull::bayes wrong size of cond, " + 
    14                 num2str(cond.length()) + ", expected size is " + num2str(dimc) ); 
    15          
    16         const vec &u = cond; // in this case cond=ut 
    17         const vec &y = yt; 
    18  
    19         vec& mu = est._mu(); 
    20         mat &P = est._R(); 
    21         mat& _Ry = fy._R(); 
    22         vec& yp = fy._mu(); 
    23         //Time update 
    24         mu = A * mu + B * u; 
    25         P  = A * P * A.transpose() + ( mat ) Q; 
    26  
    27         //Data update 
    28         _Ry = C * P * C.transpose() + ( mat ) R; 
    29         _K = P * C.transpose() * inv ( _Ry ); 
    30         P -= _K * C * P; // P = P -KCP; 
    31         yp = C * mu + D * u; 
    32         mu += _K * ( y - yp ); 
    33  
    34         if ( evalll ) { 
    35                 ll = fy.evallog ( y ); 
    36         } 
     11    bdm_assert_debug ( yt.length() == ( dimy ), "KalmanFull::bayes wrong size of dt, " + 
     12                       num2str(yt.length()) + ", expected size is " + num2str(dimy) ); 
     13    bdm_assert_debug ( cond.length() == ( dimc ), "KalmanFull::bayes wrong size of cond, " + 
     14                       num2str(cond.length()) + ", expected size is " + num2str(dimc) ); 
     15 
     16    const vec &u = cond; // in this case cond=ut 
     17    const vec &y = yt; 
     18 
     19    vec& mu = est._mu(); 
     20    mat &P = est._R(); 
     21    mat& _Ry = fy._R(); 
     22    vec& yp = fy._mu(); 
     23    //Time update 
     24    mu = A * mu + B * u; 
     25    P  = A * P * A.transpose() + ( mat ) Q; 
     26 
     27    //Data update 
     28    _Ry = C * P * C.transpose() + ( mat ) R; 
     29    _K = P * C.transpose() * inv ( _Ry ); 
     30    P -= _K * C * P; // P = P -KCP; 
     31    yp = C * mu + D * u; 
     32    mu += _K * ( y - yp ); 
     33 
     34    if ( evalll ) { 
     35        ll = fy.evallog ( y ); 
     36    } 
    3737}; 
    3838 
    … …  
    4343 
    4444void EKFfull::set_parameters ( const shared_ptr<diffbifn> &pfxu0, const shared_ptr<diffbifn> &phxu0, const mat Q0, const mat R0 ) { 
    45         pfxu = pfxu0; 
    46         phxu = phxu0; 
    47  
    48         set_dim ( pfxu0->_dimx() ); 
    49         dimy = phxu0->dimension(); 
    50         dimc = pfxu0->_dimu(); 
    51         est.set_parameters ( zeros ( dimension() ), eye ( dimension() ) ); 
    52  
    53         A.set_size ( dimension(), dimension() ); 
    54         C.set_size ( dimy, dimension() ); 
    55         //initialize matrices A C, later, these will be only updated! 
    56         pfxu->dfdx_cond ( est._mu(), zeros ( dimc ), A, true ); 
    57         B.clear(); 
    58         phxu->dfdx_cond ( est._mu(), zeros ( dimc ), C, true ); 
    59         D.clear(); 
    60  
    61         R = R0; 
    62         Q = Q0; 
     45    pfxu = pfxu0; 
     46    phxu = phxu0; 
     47 
     48    set_dim ( pfxu0->_dimx() ); 
     49    dimy = phxu0->dimension(); 
     50    dimc = pfxu0->_dimu(); 
     51    est.set_parameters ( zeros ( dimension() ), eye ( dimension() ) ); 
     52 
     53    A.set_size ( dimension(), dimension() ); 
     54    C.set_size ( dimy, dimension() ); 
     55    //initialize matrices A C, later, these will be only updated! 
     56    pfxu->dfdx_cond ( est._mu(), zeros ( dimc ), A, true ); 
     57    B.clear(); 
     58    phxu->dfdx_cond ( est._mu(), zeros ( dimc ), C, true ); 
     59    D.clear(); 
     60 
     61    R = R0; 
     62    Q = Q0; 
    6363} 
    6464 
    6565void EKFfull::bayes ( const vec &yt, const vec &cond ) { 
    66         bdm_assert_debug ( yt.length() == ( dimy ), "EKFull::bayes wrong size of dt" ); 
    67         bdm_assert_debug ( cond.length() == ( dimc ), "EKFull::bayes wrong size of dt" ); 
    68  
    69         const vec &u = cond; 
    70         const vec &y = yt; //lazy to change it 
    71         vec &mu = est._mu(); 
    72         mat &P = est._R(); 
    73         mat& _Ry = fy._R(); 
    74         vec& yp = fy._mu(); 
    75  
    76         pfxu->dfdx_cond ( mu, zeros ( dimc ), A, true ); 
    77         phxu->dfdx_cond ( mu, zeros ( dimc ), C, true ); 
    78  
    79         //Time update 
    80         mu = pfxu->eval ( mu, u );// A*mu + B*u; 
    81         P  = A * P * A.transpose() + ( mat ) Q; 
    82  
    83         //Data update 
    84         _Ry = C * P * C.transpose() + ( mat ) R; 
    85         _K = P * C.transpose() * inv ( _Ry ); 
    86         P -= _K * C * P; // P = P -KCP; 
    87         yp = phxu->eval ( mu, u ); 
    88         mu += _K * ( y - yp ); 
    89  
    90         if ( BM::evalll ) { 
    91                 ll = fy.evallog ( y ); 
    92         } 
     66    bdm_assert_debug ( yt.length() == ( dimy ), "EKFull::bayes wrong size of dt" ); 
     67    bdm_assert_debug ( cond.length() == ( dimc ), "EKFull::bayes wrong size of dt" ); 
     68 
     69    const vec &u = cond; 
     70    const vec &y = yt; //lazy to change it 
     71    vec &mu = est._mu(); 
     72    mat &P = est._R(); 
     73    mat& _Ry = fy._R(); 
     74    vec& yp = fy._mu(); 
     75 
     76    pfxu->dfdx_cond ( mu, zeros ( dimc ), A, true ); 
     77    phxu->dfdx_cond ( mu, zeros ( dimc ), C, true ); 
     78 
     79    //Time update 
     80    mu = pfxu->eval ( mu, u );// A*mu + B*u; 
     81    P  = A * P * A.transpose() + ( mat ) Q; 
     82 
     83    //Data update 
     84    _Ry = C * P * C.transpose() + ( mat ) R; 
     85    _K = P * C.transpose() * inv ( _Ry ); 
     86    P -= _K * C * P; // P = P -KCP; 
     87    yp = phxu->eval ( mu, u ); 
     88    mu += _K * ( y - yp ); 
     89 
     90    if ( BM::evalll ) { 
     91        ll = fy.evallog ( y ); 
     92    } 
    9393}; 
    9494 
    … …  
    9797void KalmanCh::set_parameters ( const mat &A0, const mat &B0, const mat &C0, const mat &D0, const chmat &Q0, const chmat &R0 ) { 
    9898 
    99         ( ( StateSpace<chmat>* ) this )->set_parameters ( A0, B0, C0, D0, Q0, R0 ); 
    100  
    101         _K = zeros ( dimension(), dimy ); 
     99    ( ( StateSpace<chmat>* ) this )->set_parameters ( A0, B0, C0, D0, Q0, R0 ); 
     100 
     101    _K = zeros ( dimension(), dimy ); 
    102102} 
    103103 
    104104void KalmanCh::initialize() { 
    105         preA = zeros ( dimy + dimension() + dimension(), dimy + dimension() ); 
     105    preA = zeros ( dimy + dimension() + dimension(), dimy + dimension() ); 
    106106//      preA.clear(); 
    107         preA.set_submatrix ( 0, 0, R._Ch() ); 
    108         preA.set_submatrix ( dimy + dimension(), dimy, Q._Ch() ); 
     107    preA.set_submatrix ( 0, 0, R._Ch() ); 
     108    preA.set_submatrix ( dimy + dimension(), dimy, Q._Ch() ); 
    109109} 
    110110 
    111111void KalmanCh::bayes ( const vec &yt, const vec &cond ) { 
    112         bdm_assert_debug ( yt.length() == dimy, "yt mismatch" ); 
    113         bdm_assert_debug ( cond.length() == dimc, "yt mismatch" ); 
    114  
    115         const vec &u = cond; 
    116         const vec &y = yt; 
    117         vec pom ( dimy ); 
    118  
    119         chmat &_P = est._R(); 
    120         vec &_mu = est._mu(); 
    121         mat _K ( dimension(), dimy ); 
    122         chmat &_Ry = fy._R(); 
    123         vec &_yp = fy._mu(); 
    124         //TODO get rid of Q in qr()! 
    125         //      mat Q; 
    126  
    127         //R and Q are already set in set_parameters() 
    128         preA.set_submatrix ( dimy, 0, ( _P._Ch() ) *C.T() ); 
    129         //Fixme can be more efficient if .T() is not used 
    130         preA.set_submatrix ( dimy, dimy, ( _P._Ch() ) *A.T() ); 
    131  
    132         if ( !qr ( preA, postA ) ) { 
    133                 bdm_warning ( "QR in KalmanCh unstable!" ); 
    134         } 
    135  
    136         ( _Ry._Ch() ) = postA ( 0, dimy - 1, 0, dimy - 1 ); 
    137         _K = inv ( A ) * ( postA ( 0, dimy - 1 , dimy, dimy + dimension() - 1 ) ).T(); 
    138         ( _P._Ch() ) = postA ( dimy, dimy + dimension() - 1, dimy, dimy + dimension() - 1 ); 
    139  
    140         _mu = A * ( _mu ) + B * u; 
    141         _yp = C * _mu - D * u; 
    142  
    143         backward_substitution ( _Ry._Ch(), ( y - _yp ), pom ); 
    144         _mu += ( _K ) * pom; 
    145  
    146         /*              cout << "P:" <<_P.to_mat() <<endl; 
    147                         cout << "Ry:" <<_Ry.to_mat() <<endl; 
    148                         cout << "_K:" <<_K <<endl;*/ 
    149  
    150         if ( evalll == true ) { //likelihood of observation y 
    151                 ll = fy.evallog ( y ); 
    152         } 
     112    bdm_assert_debug ( yt.length() == dimy, "yt mismatch" ); 
     113    bdm_assert_debug ( cond.length() == dimc, "yt mismatch" ); 
     114 
     115    const vec &u = cond; 
     116    const vec &y = yt; 
     117    vec pom ( dimy ); 
     118 
     119    chmat &_P = est._R(); 
     120    vec &_mu = est._mu(); 
     121    mat _K ( dimension(), dimy ); 
     122    chmat &_Ry = fy._R(); 
     123    vec &_yp = fy._mu(); 
     124    //TODO get rid of Q in qr()! 
     125    //  mat Q; 
     126 
     127    //R and Q are already set in set_parameters() 
     128    preA.set_submatrix ( dimy, 0, ( _P._Ch() ) *C.T() ); 
     129    //Fixme can be more efficient if .T() is not used 
     130    preA.set_submatrix ( dimy, dimy, ( _P._Ch() ) *A.T() ); 
     131 
     132    if ( !qr ( preA, postA ) ) { 
     133        bdm_warning ( "QR in KalmanCh unstable!" ); 
     134    } 
     135 
     136    ( _Ry._Ch() ) = postA ( 0, dimy - 1, 0, dimy - 1 ); 
     137    _K = inv ( A ) * ( postA ( 0, dimy - 1 , dimy, dimy + dimension() - 1 ) ).T(); 
     138    ( _P._Ch() ) = postA ( dimy, dimy + dimension() - 1, dimy, dimy + dimension() - 1 ); 
     139 
     140    _mu = A * ( _mu ) + B * u; 
     141    _yp = C * _mu - D * u; 
     142 
     143    backward_substitution ( _Ry._Ch(), ( y - _yp ), pom ); 
     144    _mu += ( _K ) * pom; 
     145 
     146    /*          cout << "P:" <<_P.to_mat() <<endl; 
     147                cout << "Ry:" <<_Ry.to_mat() <<endl; 
     148                cout << "_K:" <<_K <<endl;*/ 
     149 
     150    if ( evalll == true ) { //likelihood of observation y 
     151        ll = fy.evallog ( y ); 
     152    } 
    153153} 
    154154 
    155155void StateCanonical::connect_mlnorm ( const mlnorm<fsqmat> &ml ) { 
    156         //get ids of yrv 
    157         const RV &yrv = ml._rv(); 
    158         //need to determine u_t - it is all in _rvc that is not in ml._rv() 
    159         RV rgr0 = ml._rvc().remove_time(); 
    160         RV urv = rgr0.subt ( yrv ); 
    161  
    162         //We can do only 1d now... :( 
    163         bdm_assert ( yrv._dsize() == 1, "Only for SISO so far..." ); 
    164  
    165         // create names for 
    166         RV xrv; //empty 
    167         RV Crv; //empty 
    168         int td = ml._rvc().mint(); 
    169         // assuming strictly proper function!!! 
    170         for ( int t = -1; t >= td; t-- ) { 
    171                 xrv.add ( yrv.copy_t ( t ) ); 
    172                 Crv.add ( urv.copy_t ( t ) ); 
    173         } 
    174  
    175         // get mapp 
    176         th2A.set_connection ( xrv, ml._rvc() ); 
    177         th2C.set_connection ( Crv, ml._rvc() ); 
    178         th2D.set_connection ( urv, ml._rvc() ); 
    179  
    180         //set matrix sizes 
    181         this->A = zeros ( xrv._dsize(), xrv._dsize() ); 
    182         for ( int j = 1; j < xrv._dsize(); j++ ) { 
    183                 A ( j, j - 1 ) = 1.0;    // off diagonal 
    184         } 
    185         this->B = zeros ( xrv._dsize(), 1 ); 
    186         this->B ( 0 ) = 1.0; 
    187         this->C = zeros ( 1, xrv._dsize() ); 
    188         this->D = zeros ( 1, urv._dsize() ); 
    189         this->Q = zeros ( xrv._dsize(), xrv._dsize() ); 
    190         // R is set by update 
    191  
    192         //set cache 
    193         this->A1row = zeros ( xrv._dsize() ); 
    194         this->C1row = zeros ( xrv._dsize() ); 
    195         this->D1row = zeros ( urv._dsize() ); 
    196  
    197         update_from ( ml ); 
    198         validate(); 
     156    //get ids of yrv 
     157    const RV &yrv = ml._rv(); 
     158    //need to determine u_t - it is all in _rvc that is not in ml._rv() 
     159    RV rgr0 = ml._rvc().remove_time(); 
     160    RV urv = rgr0.subt ( yrv ); 
     161 
     162    //We can do only 1d now... :( 
     163    bdm_assert ( yrv._dsize() == 1, "Only for SISO so far..." ); 
     164 
     165    // create names for 
     166    RV xrv; //empty 
     167    RV Crv; //empty 
     168    int td = ml._rvc().mint(); 
     169    // assuming strictly proper function!!! 
     170    for ( int t = -1; t >= td; t-- ) { 
     171        xrv.add ( yrv.copy_t ( t ) ); 
     172        Crv.add ( urv.copy_t ( t ) ); 
     173    } 
     174 
     175    // get mapp 
     176    th2A.set_connection ( xrv, ml._rvc() ); 
     177    th2C.set_connection ( Crv, ml._rvc() ); 
     178    th2D.set_connection ( urv, ml._rvc() ); 
     179 
     180    //set matrix sizes 
     181    this->A = zeros ( xrv._dsize(), xrv._dsize() ); 
     182    for ( int j = 1; j < xrv._dsize(); j++ ) { 
     183        A ( j, j - 1 ) = 1.0;    // off diagonal 
     184    } 
     185    this->B = zeros ( xrv._dsize(), 1 ); 
     186    this->B ( 0 ) = 1.0; 
     187    this->C = zeros ( 1, xrv._dsize() ); 
     188    this->D = zeros ( 1, urv._dsize() ); 
     189    this->Q = zeros ( xrv._dsize(), xrv._dsize() ); 
     190    // R is set by update 
     191 
     192    //set cache 
     193    this->A1row = zeros ( xrv._dsize() ); 
     194    this->C1row = zeros ( xrv._dsize() ); 
     195    this->D1row = zeros ( urv._dsize() ); 
     196 
     197    update_from ( ml ); 
     198    validate(); 
    199199}; 
    200200 
    201201void StateCanonical::update_from ( const mlnorm<fsqmat> &ml ) { 
    202202 
    203         vec theta = ml._A().get_row ( 0 ); // this 
    204  
    205         th2A.filldown ( theta, A1row ); 
    206         th2C.filldown ( theta, C1row ); 
    207         th2D.filldown ( theta, D1row ); 
    208  
    209         R = ml._R(); 
    210  
    211         A.set_row ( 0, A1row ); 
    212         C.set_row ( 0, C1row + D1row ( 0 ) *A1row ); 
    213         D.set_row ( 0, D1row ); 
     203    vec theta = ml._A().get_row ( 0 ); // this 
     204 
     205    th2A.filldown ( theta, A1row ); 
     206    th2C.filldown ( theta, C1row ); 
     207    th2D.filldown ( theta, D1row ); 
     208 
     209    R = ml._R(); 
     210 
     211    A.set_row ( 0, A1row ); 
     212    C.set_row ( 0, C1row + D1row ( 0 ) *A1row ); 
     213    D.set_row ( 0, D1row ); 
    214214} 
    215215 
    216216void StateFromARX::connect_mlnorm ( const mlnorm<chmat> &ml, RV &xrv, RV &urv ) { 
    217         //get ids of yrv 
    218         const RV &yrv = ml._rv(); 
    219         //need to determine u_t - it is all in _rvc that is not in ml._rv() 
    220         const RV &rgr = ml._rvc(); 
    221         RV rgr0 = rgr.remove_time(); 
    222         urv = rgr0.subt ( yrv ); 
    223  
    224         // create names for state variables 
    225         xrv = yrv; 
    226  
    227         int y_multiplicity = -rgr.mint ( yrv ); 
    228         int y_block_size = yrv.length() * ( y_multiplicity ); // current yt + all delayed yts 
    229         for ( int m = 0; m < y_multiplicity - 1; m++ ) { // ========= -1 is important see arx2statespace_notes 
    230                 xrv.add ( yrv.copy_t ( -m - 1 ) ); //add delayed yt 
    231         } 
    232         //! temporary RV for connection to ml.rvc, since notation of xrv and ml.rvc does not match 
    233         RV xrv_ml = xrv.copy_t ( -1 ); 
    234  
    235         // add regressors 
    236         ivec u_block_sizes ( urv.length() ); // no_blocks = yt + unique rgr 
    237         for ( int r = 0; r < urv.length(); r++ ) { 
    238                 RV R = urv.subselect ( vec_1 ( r ) ); //non-delayed element of rgr 
    239                 int r_size = urv.size ( r ); 
    240                 int r_multiplicity = -rgr.mint ( R ); 
    241                 u_block_sizes ( r ) = r_size * r_multiplicity ; 
    242                 for ( int m = 0; m < r_multiplicity; m++ ) { 
    243                         xrv.add ( R.copy_t ( -m - 1 ) ); //add delayed yt 
    244                         xrv_ml.add ( R.copy_t ( -m - 1 ) ); //add delayed yt 
    245                 } 
    246         } 
    247         // add constant 
    248         if ( any ( ml._mu_const() != 0.0 ) ) { 
    249                 have_constant = true; 
    250                 xrv.add ( RV ( "bdm_reserved_constant_one", 1 ) ); 
    251         } else { 
    252                 have_constant = false; 
    253         } 
    254  
    255  
    256         // get mapp 
    257         th2A.set_connection ( xrv_ml, ml._rvc() ); 
    258         th2B.set_connection ( urv, ml._rvc() ); 
    259  
    260         //set matrix sizes 
    261         this->A = zeros ( xrv._dsize(), xrv._dsize() ); 
    262         //create y block 
    263         diagonal_part ( this->A, yrv._dsize(), 0, y_block_size - yrv._dsize() ); 
    264  
    265         this->B = zeros ( xrv._dsize(), urv._dsize() ); 
    266         //add diagonals for rgr 
    267         int active_x = y_block_size; 
    268         int active_Bcol = 0; 
    269         for ( int r = 0; r < urv.length(); r++ ) { 
    270                 if (u_block_sizes(r)>0){ 
    271                         diagonal_part ( this->A, active_x + urv.size ( r ), active_x, u_block_sizes ( r ) - urv.size ( r ) ); 
    272                         this->B.set_submatrix ( active_x, active_Bcol, eye ( urv.size ( r ) ) ); 
    273                         active_Bcol+=u_block_sizes(r); 
    274                 } 
    275                 active_x += u_block_sizes ( r ); 
    276         } 
    277         this->C = zeros ( yrv._dsize(), xrv._dsize() ); 
    278         this->C.set_submatrix ( 0, 0, eye ( yrv._dsize() ) ); 
    279         this->D = zeros ( yrv._dsize(), urv._dsize() ); 
    280         this->R.setCh ( zeros ( yrv._dsize(), yrv._dsize() ) ); 
    281         this->Q.setCh ( zeros ( xrv._dsize(), xrv._dsize() ) ); 
    282         // Q is set by update 
    283  
    284         update_from ( ml ); 
    285         validate(); 
    286 } 
    287          
     217    //get ids of yrv 
     218    const RV &yrv = ml._rv(); 
     219    //need to determine u_t - it is all in _rvc that is not in ml._rv() 
     220    const RV &rgr = ml._rvc(); 
     221    RV rgr0 = rgr.remove_time(); 
     222    urv = rgr0.subt ( yrv ); 
     223 
     224    // create names for state variables 
     225    xrv = yrv; 
     226 
     227    int y_multiplicity = -rgr.mint ( yrv ); 
     228    int y_block_size = yrv.length() * ( y_multiplicity ); // current yt + all delayed yts 
     229    for ( int m = 0; m < y_multiplicity - 1; m++ ) { // ========= -1 is important see arx2statespace_notes 
     230        xrv.add ( yrv.copy_t ( -m - 1 ) ); //add delayed yt 
     231    } 
     232    //! temporary RV for connection to ml.rvc, since notation of xrv and ml.rvc does not match 
     233    RV xrv_ml = xrv.copy_t ( -1 ); 
     234 
     235    // add regressors 
     236    ivec u_block_sizes ( urv.length() ); // no_blocks = yt + unique rgr 
     237    for ( int r = 0; r < urv.length(); r++ ) { 
     238        RV R = urv.subselect ( vec_1 ( r ) ); //non-delayed element of rgr 
     239        int r_size = urv.size ( r ); 
     240        int r_multiplicity = -rgr.mint ( R ); 
     241        u_block_sizes ( r ) = r_size * r_multiplicity ; 
     242        for ( int m = 0; m < r_multiplicity; m++ ) { 
     243            xrv.add ( R.copy_t ( -m - 1 ) ); //add delayed yt 
     244            xrv_ml.add ( R.copy_t ( -m - 1 ) ); //add delayed yt 
     245        } 
     246    } 
     247    // add constant 
     248    if ( any ( ml._mu_const() != 0.0 ) ) { 
     249        have_constant = true; 
     250        xrv.add ( RV ( "bdm_reserved_constant_one", 1 ) ); 
     251    } else { 
     252        have_constant = false; 
     253    } 
     254 
     255 
     256    // get mapp 
     257    th2A.set_connection ( xrv_ml, ml._rvc() ); 
     258    th2B.set_connection ( urv, ml._rvc() ); 
     259 
     260    //set matrix sizes 
     261    this->A = zeros ( xrv._dsize(), xrv._dsize() ); 
     262    //create y block 
     263    diagonal_part ( this->A, yrv._dsize(), 0, y_block_size - yrv._dsize() ); 
     264 
     265    this->B = zeros ( xrv._dsize(), urv._dsize() ); 
     266    //add diagonals for rgr 
     267    int active_x = y_block_size; 
     268    int active_Bcol = 0; 
     269    for ( int r = 0; r < urv.length(); r++ ) { 
     270        if (u_block_sizes(r)>0) { 
     271            diagonal_part ( this->A, active_x + urv.size ( r ), active_x, u_block_sizes ( r ) - urv.size ( r ) ); 
     272            this->B.set_submatrix ( active_x, active_Bcol, eye ( urv.size ( r ) ) ); 
     273            active_Bcol+=u_block_sizes(r); 
     274        } 
     275        active_x += u_block_sizes ( r ); 
     276    } 
     277    this->C = zeros ( yrv._dsize(), xrv._dsize() ); 
     278    this->C.set_submatrix ( 0, 0, eye ( yrv._dsize() ) ); 
     279    this->D = zeros ( yrv._dsize(), urv._dsize() ); 
     280    this->R.setCh ( zeros ( yrv._dsize(), yrv._dsize() ) ); 
     281    this->Q.setCh ( zeros ( xrv._dsize(), xrv._dsize() ) ); 
     282    // Q is set by update 
     283 
     284    update_from ( ml ); 
     285    validate(); 
     286} 
     287 
    288288void StateFromARX::update_from ( const mlnorm<chmat> &ml ) { 
    289         vec Arow = zeros ( A.cols() ); 
    290         vec Brow = zeros ( B.cols() ); 
    291         //  ROW- WISE EVALUATION ===== 
    292         for ( int i = 0; i < ml._rv()._dsize(); i++ ) { 
    293  
    294                 vec theta = ml._A().get_row ( i ); 
    295  
    296                 th2A.filldown ( theta, Arow ); 
    297                 if ( have_constant ) { 
    298                         // constant is always at the end no need for datalink 
    299                         Arow ( A.cols() - 1 ) = ml._mu_const() ( i ); 
    300                 } 
    301                 this->A.set_row ( i, Arow ); 
    302  
    303                 th2B.filldown ( theta, Brow ); 
    304                 this->B.set_row ( i, Brow ); 
    305         } 
    306         this->Q._Ch().set_submatrix ( 0, 0, ml.__R()._Ch() ); 
     289    vec Arow = zeros ( A.cols() ); 
     290    vec Brow = zeros ( B.cols() ); 
     291    //  ROW- WISE EVALUATION ===== 
     292    for ( int i = 0; i < ml._rv()._dsize(); i++ ) { 
     293 
     294        vec theta = ml._A().get_row ( i ); 
     295 
     296        th2A.filldown ( theta, Arow ); 
     297        if ( have_constant ) { 
     298            // constant is always at the end no need for datalink 
     299            Arow ( A.cols() - 1 ) = ml._mu_const() ( i ); 
     300        } 
     301        this->A.set_row ( i, Arow ); 
     302 
     303        th2B.filldown ( theta, Brow ); 
     304        this->B.set_row ( i, Brow ); 
     305    } 
     306    this->Q._Ch().set_submatrix ( 0, 0, ml.__R()._Ch() ); 
    307307 
    308308} 
    … …  
    310310 
    311311void EKFCh::set_parameters ( const shared_ptr<diffbifn> &pfxu0, const shared_ptr<diffbifn> &phxu0, const chmat Q0, const chmat R0 ) { 
    312         pfxu = pfxu0; 
    313         phxu = phxu0; 
    314  
    315         set_dim ( pfxu0->_dimx() ); 
    316         dimy = phxu0->dimension(); 
    317         dimc = pfxu0->_dimu(); 
    318  
    319         vec &_mu = est._mu(); 
    320         // if mu is not set, set it to zeros, just for constant terms of A and C 
    321         if ( _mu.length() != dimension() ) _mu = zeros ( dimension() ); 
    322         A = zeros ( dimension(), dimension() ); 
    323         C = zeros ( dimy, dimension() ); 
    324         preA = zeros ( dimy + dimension() + dimension(), dimy + dimension() ); 
    325  
    326         //initialize matrices A C, later, these will be only updated! 
    327         pfxu->dfdx_cond ( _mu, zeros ( dimc ), A, true ); 
     312    pfxu = pfxu0; 
     313    phxu = phxu0; 
     314 
     315    set_dim ( pfxu0->_dimx() ); 
     316    dimy = phxu0->dimension(); 
     317    dimc = pfxu0->_dimu(); 
     318 
     319    vec &_mu = est._mu(); 
     320    // if mu is not set, set it to zeros, just for constant terms of A and C 
     321    if ( _mu.length() != dimension() ) _mu = zeros ( dimension() ); 
     322    A = zeros ( dimension(), dimension() ); 
     323    C = zeros ( dimy, dimension() ); 
     324    preA = zeros ( dimy + dimension() + dimension(), dimy + dimension() ); 
     325 
     326    //initialize matrices A C, later, these will be only updated! 
     327    pfxu->dfdx_cond ( _mu, zeros ( dimc ), A, true ); 
    328328//      pfxu->dfdu_cond ( *_mu,zeros ( dimu ),B,true ); 
    329         B.clear(); 
    330         phxu->dfdx_cond ( _mu, zeros ( dimc ), C, true ); 
     329    B.clear(); 
     330    phxu->dfdx_cond ( _mu, zeros ( dimc ), C, true ); 
    331331//      phxu->dfdu_cond ( *_mu,zeros ( dimu ),D,true ); 
    332         D.clear(); 
    333  
    334         R = R0; 
    335         Q = Q0; 
    336  
    337         // Cholesky special! 
    338         preA.clear(); 
    339         preA.set_submatrix ( 0, 0, R._Ch() ); 
    340         preA.set_submatrix ( dimy + dimension(), dimy, Q._Ch() ); 
     332    D.clear(); 
     333 
     334    R = R0; 
     335    Q = Q0; 
     336 
     337    // Cholesky special! 
     338    preA.clear(); 
     339    preA.set_submatrix ( 0, 0, R._Ch() ); 
     340    preA.set_submatrix ( dimy + dimension(), dimy, Q._Ch() ); 
    341341} 
    342342 
    … …  
    344344void EKFCh::bayes ( const vec &yt, const vec &cond ) { 
    345345 
    346         vec pom ( dimy ); 
    347         const vec &u = cond; 
    348         const vec &y = yt; 
    349         vec &_mu = est._mu(); 
    350         chmat &_P = est._R(); 
    351         chmat &_Ry = fy._R(); 
    352         vec &_yp = fy._mu(); 
    353  
    354         pfxu->dfdx_cond ( _mu, u, A, false ); //update A by a derivative of fx 
    355         phxu->dfdx_cond ( _mu, u, C, false ); //update A by a derivative of fx 
    356  
    357         //R and Q are already set in set_parameters() 
    358         preA.set_submatrix ( dimy, 0, ( _P._Ch() ) *C.T() ); 
    359         //Fixme can be more efficient if .T() is not used 
    360         preA.set_submatrix ( dimy, dimy, ( _P._Ch() ) *A.T() ); 
     346    vec pom ( dimy ); 
     347    const vec &u = cond; 
     348    const vec &y = yt; 
     349    vec &_mu = est._mu(); 
     350    chmat &_P = est._R(); 
     351    chmat &_Ry = fy._R(); 
     352    vec &_yp = fy._mu(); 
     353 
     354    pfxu->dfdx_cond ( _mu, u, A, false ); //update A by a derivative of fx 
     355    phxu->dfdx_cond ( _mu, u, C, false ); //update A by a derivative of fx 
     356 
     357    //R and Q are already set in set_parameters() 
     358    preA.set_submatrix ( dimy, 0, ( _P._Ch() ) *C.T() ); 
     359    //Fixme can be more efficient if .T() is not used 
     360    preA.set_submatrix ( dimy, dimy, ( _P._Ch() ) *A.T() ); 
    361361 
    362362//      mat Sttm = _P->to_mat(); 
    … …  
    364364//      cout << "_mu:" << _mu <<endl; 
    365365 
    366         if ( !qr ( preA, postA ) ) { 
    367                 bdm_warning ( "QR in EKFCh unstable!" ); 
    368         } 
    369  
    370  
    371         ( _Ry._Ch() ) = postA ( 0, dimy - 1, 0, dimy - 1 ); 
    372         _K = inv ( A ) * ( postA ( 0, dimy - 1 , dimy, dimy + dimension() - 1 ) ).T(); 
    373         ( _P._Ch() ) = postA ( dimy, dimy + dimension() - 1, dimy, dimy + dimension() - 1 ); 
     366    if ( !qr ( preA, postA ) ) { 
     367        bdm_warning ( "QR in EKFCh unstable!" ); 
     368    } 
     369 
     370 
     371    ( _Ry._Ch() ) = postA ( 0, dimy - 1, 0, dimy - 1 ); 
     372    _K = inv ( A ) * ( postA ( 0, dimy - 1 , dimy, dimy + dimension() - 1 ) ).T(); 
     373    ( _P._Ch() ) = postA ( dimy, dimy + dimension() - 1, dimy, dimy + dimension() - 1 ); 
    374374 
    375375//      mat iRY = inv(_Ry->to_mat()); 
    … …  
    377377//      mat _K2 = Stt*C.T()*inv(R.to_mat()); 
    378378 
    379         // prediction 
    380         _mu = pfxu->eval ( _mu , u ); 
    381         _yp = phxu->eval ( _mu, u ); 
    382  
    383         /*      vec mu2 = *_mu + ( _K2 ) * ( y-*_yp );*/ 
    384  
    385         //correction //= initial value is already prediction! 
    386         backward_substitution ( _Ry._Ch(), ( y - _yp ), pom ); 
    387         _mu += ( _K ) * pom ; 
    388  
    389         /*      _K = (_P->to_mat())*C.transpose() * ( _iRy->to_mat() ); 
    390                 *_mu = pfxu->eval ( *_mu ,u ) + ( _K )* ( y-*_yp );*/ 
     379    // prediction 
     380    _mu = pfxu->eval ( _mu , u ); 
     381    _yp = phxu->eval ( _mu, u ); 
     382 
     383    /*  vec mu2 = *_mu + ( _K2 ) * ( y-*_yp );*/ 
     384 
     385    //correction //= initial value is already prediction! 
     386    backward_substitution ( _Ry._Ch(), ( y - _yp ), pom ); 
     387    _mu += ( _K ) * pom ; 
     388 
     389    /*  _K = (_P->to_mat())*C.transpose() * ( _iRy->to_mat() ); 
     390        *_mu = pfxu->eval ( *_mu ,u ) + ( _K )* ( y-*_yp );*/ 
    391391 
    392392//              cout << "P:" <<_P.to_mat() <<endl; 
    … …  
    395395//      cout << "dt:" <<dt <<endl; 
    396396 
    397         if ( evalll == true ) { //likelihood of observation y 
    398                 ll = fy.evallog ( y ); 
    399         } 
     397    if ( evalll == true ) { //likelihood of observation y 
     398        ll = fy.evallog ( y ); 
     399    } 
    400400} 
    401401 
    402402void EKFCh::from_setting ( const Setting &set ) { 
    403         BM::from_setting ( set ); 
    404         shared_ptr<diffbifn> IM = UI::build<diffbifn> ( set, "IM", UI::compulsory ); 
    405         shared_ptr<diffbifn> OM = UI::build<diffbifn> ( set, "OM", UI::compulsory ); 
    406  
    407         //statistics 
    408         int dim = IM->dimension(); 
    409         vec mu0; 
    410         if ( !UI::get ( mu0, set, "mu0" ) ) 
    411                 mu0 = zeros ( dim ); 
    412  
    413         mat P0; 
    414         vec dP0; 
    415         if ( UI::get ( dP0, set, "dP0" ) ) 
    416                 P0 = diag ( dP0 ); 
    417         else if ( !UI::get ( P0, set, "P0" ) ) 
    418                 P0 = eye ( dim ); 
    419  
    420         set_statistics ( mu0, P0 ); 
    421  
    422         //parameters 
    423         vec dQ, dR; 
    424         UI::get ( dQ, set, "dQ", UI::compulsory ); 
    425         UI::get ( dR, set, "dR", UI::compulsory ); 
    426         set_parameters ( IM, OM, diag ( dQ ), diag ( dR ) ); 
     403    BM::from_setting ( set ); 
     404    shared_ptr<diffbifn> IM = UI::build<diffbifn> ( set, "IM", UI::compulsory ); 
     405    shared_ptr<diffbifn> OM = UI::build<diffbifn> ( set, "OM", UI::compulsory ); 
     406 
     407    //statistics 
     408    int dim = IM->dimension(); 
     409    vec mu0; 
     410    if ( !UI::get ( mu0, set, "mu0" ) ) 
     411        mu0 = zeros ( dim ); 
     412 
     413    mat P0; 
     414    vec dP0; 
     415    if ( UI::get ( dP0, set, "dP0" ) ) 
     416        P0 = diag ( dP0 ); 
     417    else if ( !UI::get ( P0, set, "P0" ) ) 
     418        P0 = eye ( dim ); 
     419 
     420    set_statistics ( mu0, P0 ); 
     421 
     422    //parameters 
     423    vec dQ, dR; 
     424    UI::get ( dQ, set, "dQ", UI::compulsory ); 
     425    UI::get ( dR, set, "dR", UI::compulsory ); 
     426    set_parameters ( IM, OM, diag ( dQ ), diag ( dR ) ); 
    427427} 
    428428 
    429429void MultiModel::from_setting ( const Setting &set ) { 
    430         Array<EKFCh*> A; 
    431         UI::get ( A, set, "models", UI::compulsory ); 
    432  
    433         set_parameters ( A ); 
    434         set_yrv ( A ( 0 )->_yrv() ); 
    435         //set_rv(A(0)->_rv()); 
    436 } 
    437  
    438 } 
     430    Array<EKFCh*> A; 
     431    UI::get ( A, set, "models", UI::compulsory ); 
     432 
     433    set_parameters ( A ); 
     434    set_yrv ( A ( 0 )->_yrv() ); 
     435    //set_rv(A(0)->_rv()); 
     436} 
     437 
     438}