| 61 | void KalmanCh::set_parameters ( const mat &A0,const mat &B0,const mat &C0,const mat &D0,const chmat &R0,const chmat &Q0 ) { |
| 62 | |
| 63 | ( ( Kalman<chmat>* ) this )->set_parameters ( A0,B0,C0,D0,R0,Q0 ); |
| 64 | // Cholesky special! |
| 65 | preA.clear(); |
| 66 | preA.set_submatrix ( 0,0,R._Ch() ); |
| 67 | preA.set_submatrix ( dimy+dimx,dimy,Q._Ch() ); |
| 68 | } |
| 69 | |
| 70 | |
| 71 | void KalmanCh::bayes ( const vec &dt ) { |
| 72 | |
| 73 | vec u = dt.get ( dimy,dimy+dimu-1 ); |
| 74 | vec y = dt.get ( 0,dimy-1 ); |
| 75 | |
| 76 | //TODO get rid of Q in qr()! |
| 77 | // mat Q; |
| 78 | |
| 79 | //R and Q are already set in set_parameters() |
| 80 | preA.set_submatrix ( dimy,0, ( _P->_Ch() ) *C.T() ); |
| 81 | //Fixme can be more efficient if .T() is not used |
| 82 | preA.set_submatrix ( dimy,dimy, ( _P->_Ch() ) *A.T() ); |
| 83 | |
| 84 | // if ( !qr ( preA,Q,postA ) ) it_warning ( "QR in kalman unstable!" ); |
| 85 | if ( !qr ( preA,postA ) ) it_warning ( "QR in kalman unstable!" ); |
| 86 | |
| 87 | ( _Ry->_Ch() ) =postA ( 0,dimy-1, 0,dimy-1 ); |
| 88 | _K = ( postA ( 0,dimy-1 ,dimy,dimy+dimx-1 ) ).T(); |
| 89 | ( _P->_Ch() ) = postA ( dimy,dimy+dimx-1,dimy,dimy+dimx-1 ); |
| 90 | _Ry->inv ( *_iRy ); |
| 91 | fy._cached ( true ); |
| 92 | *_mu = A* ( *_mu ) + B*u + ( _K ) * ( _iRy->_Ch() ).T() * ( y-C* ( *_mu )-D*u ); |
| 93 | |
| 94 | /* cout << "P:" <<_P->to_mat() <<endl; |
| 95 | cout << "Ry:" <<_Ry->to_mat() <<endl; |
| 96 | cout << "iRy:" <<_iRy->to_mat() <<endl;*/ |
| 97 | if ( evalll==true ) { //likelihood of observation y |
| 98 | ll=fy.evalpdflog ( y ); |
| 99 | } |
| 100 | } |
| 101 | |
| 102 | |
| 103 | EKFCh::EKFCh ( RV rvx0, RV rvy0, RV rvu0 ) : KalmanCh ( rvx0,rvy0,rvu0 ) {} |
| 104 | |
| 105 | void EKFCh::set_parameters ( diffbifn* pfxu0, diffbifn* phxu0,const chmat Q0,const chmat R0 ) { |
| 106 | pfxu = pfxu0; |
| 107 | phxu = phxu0; |
| 108 | |
| 109 | //initialize matrices A C, later, these will be only updated! |
| 110 | pfxu->dfdx_cond ( *_mu,zeros ( dimu ),A,true ); |
| 111 | // pfxu->dfdu_cond ( *_mu,zeros ( dimu ),B,true ); |
| 112 | B.clear(); |
| 113 | phxu->dfdx_cond ( *_mu,zeros ( dimu ),C,true ); |
| 114 | // phxu->dfdu_cond ( *_mu,zeros ( dimu ),D,true ); |
| 115 | D.clear(); |
| 116 | |
| 117 | R = R0; |
| 118 | Q = Q0; |
| 119 | |
| 120 | // Cholesky special! |
| 121 | preA.clear(); |
| 122 | preA.set_submatrix ( 0,0,R._Ch() ); |
| 123 | preA.set_submatrix ( dimy+dimx,dimy,Q._Ch() ); |
| 124 | } |
| 125 | |
| 126 | |
| 127 | void EKFCh::bayes ( const vec &dt ) { |
| 128 | |
| 129 | vec u = dt.get ( dimy,dimy+dimu-1 ); |
| 130 | vec y = dt.get ( 0,dimy-1 ); |
| 131 | |
| 132 | //TODO get rid of Q in qr()! |
| 133 | // mat Q; |
| 134 | |
| 135 | //R and Q are already set in set_parameters() |
| 136 | preA.set_submatrix ( dimy,0, ( _P->_Ch() ) *C.T() ); |
| 137 | //Fixme can be more efficient if .T() is not used |
| 138 | preA.set_submatrix ( dimy,dimy, ( _P->_Ch() ) *A.T() ); |
| 139 | |
| 140 | // if ( !qr ( preA,Q,postA ) ) it_warning ( "QR in kalman unstable!" ); |
| 141 | if ( !qr ( preA,postA ) ) it_warning ( "QR in kalman unstable!" ); |
| 142 | |
| 143 | ( _Ry->_Ch() ) =postA ( 0,dimy-1, 0,dimy-1 ); |
| 144 | _K = ( postA ( 0,dimy-1 ,dimy,dimy+dimx-1 ) ).T(); |
| 145 | ( _P->_Ch() ) = postA ( dimy,dimy+dimx-1,dimy,dimy+dimx-1 ); |
| 146 | _Ry->inv ( *_iRy ); |
| 147 | fy._cached ( true ); |
| 148 | *_yp = phxu->eval ( *_mu,u ); |
| 149 | *_mu = pfxu->eval ( *_mu ,u ) + ( _K ) * ( _iRy->_Ch() ).T() * ( y-*_yp ); |
| 150 | |
| 151 | /* cout << "P:" <<_P->to_mat() <<endl; |
| 152 | cout << "Ry:" <<_Ry->to_mat() <<endl; |
| 153 | cout << "iRy:" <<_iRy->to_mat() <<endl;*/ |
| 154 | |
| 155 | if ( evalll==true ) { //likelihood of observation y |
| 156 | ll=fy.evalpdflog ( y ); |
| 157 | } |
| 158 | } |
| 159 | |