[262] | 1 | |
---|
[7] | 2 | #include "libKF.h" |
---|
| 3 | |
---|
[254] | 4 | namespace bdm{ |
---|
[7] | 5 | |
---|
| 6 | using std::endl; |
---|
| 7 | |
---|
[32] | 8 | KalmanFull::KalmanFull ( mat A0, mat B0, mat C0, mat D0, mat R0, mat Q0, mat P0, vec mu0 ) { |
---|
[7] | 9 | dimx = A0.rows(); |
---|
| 10 | dimu = B0.cols(); |
---|
| 11 | dimy = C0.rows(); |
---|
| 12 | |
---|
[32] | 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 ( ( R0.cols() ==dimy ) || ( R0.rows() ==dimy ), "KalmanFull: R is not compatible" ); |
---|
| 18 | it_assert_debug ( ( Q0.cols() ==dimx ) || ( Q0.rows() ==dimx ), "KalmanFull: Q is not compatible" ); |
---|
[7] | 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; |
---|
[37] | 28 | |
---|
| 29 | ll = 0.0; |
---|
| 30 | evalll=true; |
---|
[7] | 31 | } |
---|
| 32 | |
---|
[32] | 33 | void KalmanFull::bayes ( const vec &dt ) { |
---|
| 34 | it_assert_debug ( dt.length() == ( dimy+dimu ),"KalmanFull::bayes wrong size of dt" ); |
---|
[7] | 35 | |
---|
[32] | 36 | vec u = dt.get ( dimy,dimy+dimu-1 ); |
---|
| 37 | vec y = dt.get ( 0,dimy-1 ); |
---|
[7] | 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; |
---|
[32] | 44 | _iRy = inv ( _Ry ); |
---|
| 45 | _K = P*C.transpose() *_iRy; |
---|
[7] | 46 | P -= _K*C*P; // P = P -KCP; |
---|
[32] | 47 | mu += _K* ( y-C*mu-D*u ); |
---|
[37] | 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 | } |
---|
[7] | 54 | }; |
---|
| 55 | |
---|
| 56 | std::ostream &operator<< ( std::ostream &os, const KalmanFull &kf ) { |
---|
| 57 | os << "mu:" << kf.mu << endl << "P:" << kf.P <<endl; |
---|
[27] | 58 | return os; |
---|
[7] | 59 | } |
---|
| 60 | |
---|
[62] | 61 | |
---|
| 62 | |
---|
| 63 | /////////////////////////////// EKFS |
---|
[67] | 64 | EKFfull::EKFfull ( RV rvx0, RV rvy0, RV rvu0 ) : BM ( rvx0 ),E(rvx0) {}; |
---|
[62] | 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 = rv.count(); |
---|
| 71 | dimy = phxu0->_dimy(); |
---|
[231] | 72 | dimu = pfxu0->_dimu(); |
---|
[62] | 73 | |
---|
| 74 | A.set_size(dimx,dimx); |
---|
| 75 | C.set_size(dimy,dimx); |
---|
| 76 | //initialize matrices A C, later, these will be only updated! |
---|
| 77 | pfxu->dfdx_cond ( mu,zeros ( dimu ),A,true ); |
---|
| 78 | B.clear(); |
---|
| 79 | phxu->dfdx_cond ( mu,zeros ( dimu ),C,true ); |
---|
| 80 | D.clear(); |
---|
| 81 | |
---|
| 82 | R = R0; |
---|
| 83 | Q = Q0; |
---|
| 84 | } |
---|
| 85 | |
---|
| 86 | void EKFfull::bayes ( const vec &dt ) { |
---|
| 87 | it_assert_debug ( dt.length() == ( dimy+dimu ),"KalmanFull::bayes wrong size of dt" ); |
---|
| 88 | |
---|
| 89 | vec u = dt.get ( dimy,dimy+dimu-1 ); |
---|
| 90 | vec y = dt.get ( 0,dimy-1 ); |
---|
| 91 | |
---|
| 92 | pfxu->dfdx_cond ( mu,zeros ( dimu ),A,true ); |
---|
| 93 | phxu->dfdx_cond ( mu,zeros ( dimu ),C,true ); |
---|
| 94 | |
---|
| 95 | //Time update |
---|
| 96 | mu = pfxu->eval(mu,u);// A*mu + B*u; |
---|
| 97 | P = A*P*A.transpose() + Q; |
---|
| 98 | |
---|
| 99 | //Data update |
---|
| 100 | _Ry = C*P*C.transpose() + R; |
---|
| 101 | _iRy = inv ( _Ry ); |
---|
| 102 | _K = P*C.transpose() *_iRy; |
---|
| 103 | P -= _K*C*P; // P = P -KCP; |
---|
| 104 | vec yp = phxu->eval(mu,u); |
---|
| 105 | mu += _K* ( y-yp ); |
---|
| 106 | |
---|
[67] | 107 | E.set_mu(mu); |
---|
| 108 | |
---|
[62] | 109 | if ( BM::evalll ) { |
---|
| 110 | //from enorm |
---|
| 111 | vec x=y-yp; |
---|
| 112 | BM::ll = -0.5* ( R.cols() * 1.83787706640935 +log ( det ( _Ry ) ) +x* ( _iRy*x ) ); |
---|
| 113 | } |
---|
| 114 | }; |
---|
| 115 | |
---|
| 116 | |
---|
| 117 | |
---|
[37] | 118 | void KalmanCh::set_parameters ( const mat &A0,const mat &B0,const mat &C0,const mat &D0,const chmat &R0,const chmat &Q0 ) { |
---|
| 119 | |
---|
| 120 | ( ( Kalman<chmat>* ) this )->set_parameters ( A0,B0,C0,D0,R0,Q0 ); |
---|
| 121 | // Cholesky special! |
---|
| 122 | preA.clear(); |
---|
| 123 | preA.set_submatrix ( 0,0,R._Ch() ); |
---|
| 124 | preA.set_submatrix ( dimy+dimx,dimy,Q._Ch() ); |
---|
| 125 | } |
---|
| 126 | |
---|
| 127 | |
---|
| 128 | void KalmanCh::bayes ( const vec &dt ) { |
---|
| 129 | |
---|
| 130 | vec u = dt.get ( dimy,dimy+dimu-1 ); |
---|
| 131 | vec y = dt.get ( 0,dimy-1 ); |
---|
[83] | 132 | vec pom(dimy); |
---|
| 133 | |
---|
[37] | 134 | //TODO get rid of Q in qr()! |
---|
| 135 | // mat Q; |
---|
| 136 | |
---|
| 137 | //R and Q are already set in set_parameters() |
---|
[83] | 138 | preA.set_submatrix ( dimy,0, ( _P._Ch() ) *C.T() ); |
---|
[37] | 139 | //Fixme can be more efficient if .T() is not used |
---|
[83] | 140 | preA.set_submatrix ( dimy,dimy, ( _P._Ch() ) *A.T() ); |
---|
[37] | 141 | |
---|
| 142 | // if ( !qr ( preA,Q,postA ) ) it_warning ( "QR in kalman unstable!" ); |
---|
[141] | 143 | if ( !qr ( preA,postA ) ) {it_warning ( "QR in kalman unstable!" );} |
---|
[37] | 144 | |
---|
[83] | 145 | ( _Ry._Ch() ) =postA ( 0,dimy-1, 0,dimy-1 ); |
---|
| 146 | _K = inv(A)*( postA ( 0,dimy-1 ,dimy,dimy+dimx-1 ) ).T(); |
---|
| 147 | ( _P._Ch() ) = postA ( dimy,dimy+dimx-1,dimy,dimy+dimx-1 ); |
---|
| 148 | |
---|
| 149 | _mu = A* ( _mu ) + B*u; |
---|
| 150 | _yp = C*_mu -D*u; |
---|
| 151 | |
---|
| 152 | backward_substitution(_Ry._Ch(),( y-_yp),pom); |
---|
| 153 | _mu += ( _K ) * pom; |
---|
[37] | 154 | |
---|
[215] | 155 | /* cout << "P:" <<_P.to_mat() <<endl; |
---|
[83] | 156 | cout << "Ry:" <<_Ry.to_mat() <<endl; |
---|
[215] | 157 | cout << "_K:" <<_K <<endl;*/ |
---|
[83] | 158 | |
---|
[37] | 159 | if ( evalll==true ) { //likelihood of observation y |
---|
[211] | 160 | ll=fy.evallog ( y ); |
---|
[37] | 161 | } |
---|
| 162 | } |
---|
| 163 | |
---|
| 164 | |
---|
| 165 | EKFCh::EKFCh ( RV rvx0, RV rvy0, RV rvu0 ) : KalmanCh ( rvx0,rvy0,rvu0 ) {} |
---|
| 166 | |
---|
| 167 | void EKFCh::set_parameters ( diffbifn* pfxu0, diffbifn* phxu0,const chmat Q0,const chmat R0 ) { |
---|
| 168 | pfxu = pfxu0; |
---|
| 169 | phxu = phxu0; |
---|
| 170 | |
---|
| 171 | //initialize matrices A C, later, these will be only updated! |
---|
[83] | 172 | pfxu->dfdx_cond ( _mu,zeros ( dimu ),A,true ); |
---|
[37] | 173 | // pfxu->dfdu_cond ( *_mu,zeros ( dimu ),B,true ); |
---|
| 174 | B.clear(); |
---|
[83] | 175 | phxu->dfdx_cond ( _mu,zeros ( dimu ),C,true ); |
---|
[37] | 176 | // phxu->dfdu_cond ( *_mu,zeros ( dimu ),D,true ); |
---|
| 177 | D.clear(); |
---|
| 178 | |
---|
| 179 | R = R0; |
---|
| 180 | Q = Q0; |
---|
| 181 | |
---|
| 182 | // Cholesky special! |
---|
| 183 | preA.clear(); |
---|
| 184 | preA.set_submatrix ( 0,0,R._Ch() ); |
---|
| 185 | preA.set_submatrix ( dimy+dimx,dimy,Q._Ch() ); |
---|
| 186 | } |
---|
| 187 | |
---|
| 188 | |
---|
| 189 | void EKFCh::bayes ( const vec &dt ) { |
---|
| 190 | |
---|
[83] | 191 | vec pom(dimy); |
---|
[37] | 192 | vec u = dt.get ( dimy,dimy+dimu-1 ); |
---|
| 193 | vec y = dt.get ( 0,dimy-1 ); |
---|
| 194 | |
---|
[83] | 195 | pfxu->dfdx_cond ( _mu,u,A,false ); //update A by a derivative of fx |
---|
| 196 | phxu->dfdx_cond ( _mu,u,C,false ); //update A by a derivative of fx |
---|
[37] | 197 | |
---|
| 198 | //R and Q are already set in set_parameters() |
---|
[83] | 199 | preA.set_submatrix ( dimy,0, ( _P._Ch() ) *C.T() ); |
---|
[37] | 200 | //Fixme can be more efficient if .T() is not used |
---|
[83] | 201 | preA.set_submatrix ( dimy,dimy, ( _P._Ch() ) *A.T() ); |
---|
[37] | 202 | |
---|
[62] | 203 | // mat Sttm = _P->to_mat(); |
---|
[225] | 204 | // cout << preA <<endl; |
---|
| 205 | // cout << "_mu:" << _mu <<endl; |
---|
[62] | 206 | |
---|
[37] | 207 | // if ( !qr ( preA,Q,postA ) ) it_warning ( "QR in kalman unstable!" ); |
---|
[141] | 208 | if ( !qr ( preA,postA ) ) {it_warning ( "QR in kalman unstable!" );} |
---|
[37] | 209 | |
---|
[225] | 210 | |
---|
[83] | 211 | ( _Ry._Ch() ) =postA ( 0,dimy-1, 0,dimy-1 ); |
---|
[62] | 212 | _K = inv(A)*( postA ( 0,dimy-1 ,dimy,dimy+dimx-1 ) ).T(); |
---|
[83] | 213 | ( _P._Ch() ) = postA ( dimy,dimy+dimx-1,dimy,dimy+dimx-1 ); |
---|
[62] | 214 | |
---|
| 215 | // mat iRY = inv(_Ry->to_mat()); |
---|
| 216 | // mat Stt = Sttm - Sttm * C.T() * iRY * C * Sttm; |
---|
| 217 | // mat _K2 = Stt*C.T()*inv(R.to_mat()); |
---|
| 218 | |
---|
| 219 | // prediction |
---|
[83] | 220 | _mu = pfxu->eval ( _mu ,u ); |
---|
| 221 | _yp = phxu->eval ( _mu,u ); |
---|
[51] | 222 | |
---|
[62] | 223 | /* vec mu2 = *_mu + ( _K2 ) * ( y-*_yp );*/ |
---|
| 224 | |
---|
| 225 | //correction //= initial value is already prediction! |
---|
[83] | 226 | backward_substitution(_Ry._Ch(),( y-_yp ),pom); |
---|
| 227 | _mu += ( _K ) *pom ; |
---|
[62] | 228 | |
---|
[51] | 229 | /* _K = (_P->to_mat())*C.transpose() * ( _iRy->to_mat() ); |
---|
| 230 | *_mu = pfxu->eval ( *_mu ,u ) + ( _K )* ( y-*_yp );*/ |
---|
| 231 | |
---|
[215] | 232 | // cout << "P:" <<_P.to_mat() <<endl; |
---|
| 233 | // cout << "Ry:" <<_Ry.to_mat() <<endl; |
---|
[225] | 234 | // cout << "_mu:" <<_mu <<endl; |
---|
| 235 | // cout << "dt:" <<dt <<endl; |
---|
[37] | 236 | |
---|
| 237 | if ( evalll==true ) { //likelihood of observation y |
---|
[211] | 238 | ll=fy.evallog ( y ); |
---|
[37] | 239 | } |
---|
| 240 | } |
---|
| 241 | |
---|
[32] | 242 | void KFcondQR::condition ( const vec &QR ) { |
---|
| 243 | it_assert_debug ( QR.length() == ( rvc.count() ),"KFcondRQ: conditioning by incompatible vector" ); |
---|
| 244 | |
---|
[37] | 245 | Q.setD ( QR ( 0, dimx-1 ) ); |
---|
| 246 | R.setD ( QR ( dimx, -1 ) ); |
---|
[32] | 247 | }; |
---|
| 248 | |
---|
| 249 | void KFcondR::condition ( const vec &R0 ) { |
---|
| 250 | it_assert_debug ( R0.length() == ( rvc.count() ),"KFcondR: conditioning by incompatible vector" ); |
---|
| 251 | |
---|
| 252 | R.setD ( R0 ); |
---|
| 253 | }; |
---|
[62] | 254 | |
---|
| 255 | |
---|
[262] | 256 | } |
---|