[262] | 1 | |
---|
[384] | 2 | #include "kalman.h" |
---|
[7] | 3 | |
---|
[477] | 4 | namespace bdm { |
---|
[7] | 5 | |
---|
| 6 | using std::endl; |
---|
| 7 | |
---|
| 8 | |
---|
| 9 | |
---|
[32] | 10 | void KalmanFull::bayes ( const vec &dt ) { |
---|
[565] | 11 | bdm_assert_debug ( dt.length() == ( dimy + dimu ), "KalmanFull::bayes wrong size of dt" ); |
---|
[7] | 12 | |
---|
[477] | 13 | vec u = dt.get ( dimy, dimy + dimu - 1 ); |
---|
| 14 | vec y = dt.get ( 0, dimy - 1 ); |
---|
[583] | 15 | vec& mu = est->_mu(); |
---|
| 16 | mat &P = est->_R(); |
---|
| 17 | mat& _Ry = fy._R(); |
---|
[653] | 18 | vec& yp = fy._mu(); |
---|
[7] | 19 | //Time update |
---|
[477] | 20 | mu = A * mu + B * u; |
---|
[583] | 21 | P = A * P * A.transpose() + (mat)Q; |
---|
[7] | 22 | |
---|
| 23 | //Data update |
---|
[583] | 24 | _Ry = C * P * C.transpose() + (mat)R; |
---|
| 25 | _K = P * C.transpose() * inv ( _Ry ); |
---|
[477] | 26 | P -= _K * C * P; // P = P -KCP; |
---|
[653] | 27 | yp = C * mu + D * u; |
---|
| 28 | mu += _K * ( y - yp ); |
---|
[37] | 29 | |
---|
| 30 | if ( evalll ) { |
---|
[653] | 31 | ll=fy.evallog(y); |
---|
[37] | 32 | } |
---|
[7] | 33 | }; |
---|
| 34 | |
---|
| 35 | |
---|
[62] | 36 | |
---|
[477] | 37 | /////////////////////////////// EKFS |
---|
[583] | 38 | EKFfull::EKFfull ( ) : KalmanFull () {}; |
---|
[62] | 39 | |
---|
[527] | 40 | void EKFfull::set_parameters ( const shared_ptr<diffbifn> &pfxu0, const shared_ptr<diffbifn> &phxu0, const mat Q0, const mat R0 ) { |
---|
[62] | 41 | pfxu = pfxu0; |
---|
| 42 | phxu = phxu0; |
---|
| 43 | |
---|
[270] | 44 | dimx = pfxu0->_dimx(); |
---|
[283] | 45 | dimy = phxu0->dimension(); |
---|
[231] | 46 | dimu = pfxu0->_dimu(); |
---|
[583] | 47 | est->set_parameters( zeros(dimx), eye(dimx) ); |
---|
[477] | 48 | |
---|
| 49 | A.set_size ( dimx, dimx ); |
---|
| 50 | C.set_size ( dimy, dimx ); |
---|
[62] | 51 | //initialize matrices A C, later, these will be only updated! |
---|
[583] | 52 | pfxu->dfdx_cond ( est->_mu(), zeros ( dimu ), A, true ); |
---|
[62] | 53 | B.clear(); |
---|
[583] | 54 | phxu->dfdx_cond ( est->_mu(), zeros ( dimu ), C, true ); |
---|
[62] | 55 | D.clear(); |
---|
| 56 | |
---|
| 57 | R = R0; |
---|
| 58 | Q = Q0; |
---|
| 59 | } |
---|
| 60 | |
---|
| 61 | void EKFfull::bayes ( const vec &dt ) { |
---|
[565] | 62 | bdm_assert_debug ( dt.length() == ( dimy + dimu ), "EKFull::bayes wrong size of dt" ); |
---|
[62] | 63 | |
---|
[477] | 64 | vec u = dt.get ( dimy, dimy + dimu - 1 ); |
---|
| 65 | vec y = dt.get ( 0, dimy - 1 ); |
---|
[583] | 66 | vec &mu = est->_mu(); |
---|
| 67 | mat &P = est->_R(); |
---|
| 68 | mat& _Ry = fy._R(); |
---|
[653] | 69 | vec& yp = fy._mu(); |
---|
[583] | 70 | |
---|
[477] | 71 | pfxu->dfdx_cond ( mu, zeros ( dimu ), A, true ); |
---|
| 72 | phxu->dfdx_cond ( mu, zeros ( dimu ), C, true ); |
---|
| 73 | |
---|
[62] | 74 | //Time update |
---|
[477] | 75 | mu = pfxu->eval ( mu, u );// A*mu + B*u; |
---|
[583] | 76 | P = A * P * A.transpose() + (mat)Q; |
---|
[62] | 77 | |
---|
| 78 | //Data update |
---|
[583] | 79 | _Ry = C * P * C.transpose() + (mat)R; |
---|
| 80 | _K = P * C.transpose() * inv ( _Ry ); |
---|
[477] | 81 | P -= _K * C * P; // P = P -KCP; |
---|
[653] | 82 | yp = phxu->eval ( mu, u ); |
---|
[477] | 83 | mu += _K * ( y - yp ); |
---|
[62] | 84 | |
---|
| 85 | if ( BM::evalll ) { |
---|
[583] | 86 | ll=fy.evallog(y); |
---|
[62] | 87 | } |
---|
| 88 | }; |
---|
| 89 | |
---|
| 90 | |
---|
| 91 | |
---|
[477] | 92 | void KalmanCh::set_parameters ( const mat &A0, const mat &B0, const mat &C0, const mat &D0, const chmat &Q0, const chmat &R0 ) { |
---|
[37] | 93 | |
---|
[583] | 94 | ( ( StateSpace<chmat>* ) this )->set_parameters ( A0, B0, C0, D0, Q0, R0 ); |
---|
| 95 | |
---|
| 96 | _K=zeros(dimx,dimy); |
---|
[37] | 97 | // Cholesky special! |
---|
[583] | 98 | initialize(); |
---|
| 99 | } |
---|
| 100 | |
---|
| 101 | void KalmanCh::initialize(){ |
---|
[477] | 102 | preA = zeros ( dimy + dimx + dimx, dimy + dimx ); |
---|
[271] | 103 | // preA.clear(); |
---|
[477] | 104 | preA.set_submatrix ( 0, 0, R._Ch() ); |
---|
| 105 | preA.set_submatrix ( dimy + dimx, dimy, Q._Ch() ); |
---|
[37] | 106 | } |
---|
| 107 | |
---|
| 108 | void KalmanCh::bayes ( const vec &dt ) { |
---|
| 109 | |
---|
[477] | 110 | vec u = dt.get ( dimy, dimy + dimu - 1 ); |
---|
| 111 | vec y = dt.get ( 0, dimy - 1 ); |
---|
| 112 | vec pom ( dimy ); |
---|
| 113 | |
---|
[583] | 114 | chmat &_P=est->_R(); |
---|
| 115 | vec &_mu = est->_mu(); |
---|
| 116 | mat _K(dimx,dimy); |
---|
| 117 | chmat &_Ry=fy._R(); |
---|
| 118 | vec &_yp = fy._mu(); |
---|
[37] | 119 | //TODO get rid of Q in qr()! |
---|
| 120 | // mat Q; |
---|
| 121 | |
---|
| 122 | //R and Q are already set in set_parameters() |
---|
[477] | 123 | preA.set_submatrix ( dimy, 0, ( _P._Ch() ) *C.T() ); |
---|
[37] | 124 | //Fixme can be more efficient if .T() is not used |
---|
[477] | 125 | preA.set_submatrix ( dimy, dimy, ( _P._Ch() ) *A.T() ); |
---|
[37] | 126 | |
---|
[477] | 127 | if ( !qr ( preA, postA ) ) { |
---|
[565] | 128 | bdm_warning ( "QR in KalmanCh unstable!" ); |
---|
[477] | 129 | } |
---|
[37] | 130 | |
---|
[477] | 131 | ( _Ry._Ch() ) = postA ( 0, dimy - 1, 0, dimy - 1 ); |
---|
| 132 | _K = inv ( A ) * ( postA ( 0, dimy - 1 , dimy, dimy + dimx - 1 ) ).T(); |
---|
| 133 | ( _P._Ch() ) = postA ( dimy, dimy + dimx - 1, dimy, dimy + dimx - 1 ); |
---|
[37] | 134 | |
---|
[477] | 135 | _mu = A * ( _mu ) + B * u; |
---|
| 136 | _yp = C * _mu - D * u; |
---|
| 137 | |
---|
| 138 | backward_substitution ( _Ry._Ch(), ( y - _yp ), pom ); |
---|
| 139 | _mu += ( _K ) * pom; |
---|
| 140 | |
---|
| 141 | /* cout << "P:" <<_P.to_mat() <<endl; |
---|
| 142 | cout << "Ry:" <<_Ry.to_mat() <<endl; |
---|
| 143 | cout << "_K:" <<_K <<endl;*/ |
---|
| 144 | |
---|
| 145 | if ( evalll == true ) { //likelihood of observation y |
---|
| 146 | ll = fy.evallog ( y ); |
---|
[37] | 147 | } |
---|
| 148 | } |
---|
| 149 | |
---|
| 150 | |
---|
| 151 | |
---|
[527] | 152 | void EKFCh::set_parameters ( const shared_ptr<diffbifn> &pfxu0, const shared_ptr<diffbifn> &phxu0, const chmat Q0, const chmat R0 ) { |
---|
[37] | 153 | pfxu = pfxu0; |
---|
| 154 | phxu = phxu0; |
---|
| 155 | |
---|
[283] | 156 | dimx = pfxu0->dimension(); |
---|
| 157 | dimy = phxu0->dimension(); |
---|
[279] | 158 | dimu = pfxu0->_dimu(); |
---|
[583] | 159 | |
---|
| 160 | vec &_mu = est->_mu(); |
---|
[477] | 161 | // if mu is not set, set it to zeros, just for constant terms of A and C |
---|
[583] | 162 | if ( _mu.length() != dimx ) _mu = zeros ( dimx ); |
---|
[477] | 163 | A = zeros ( dimx, dimx ); |
---|
| 164 | C = zeros ( dimy, dimx ); |
---|
| 165 | preA = zeros ( dimy + dimx + dimx, dimy + dimx ); |
---|
| 166 | |
---|
[37] | 167 | //initialize matrices A C, later, these will be only updated! |
---|
[477] | 168 | pfxu->dfdx_cond ( _mu, zeros ( dimu ), A, true ); |
---|
[37] | 169 | // pfxu->dfdu_cond ( *_mu,zeros ( dimu ),B,true ); |
---|
| 170 | B.clear(); |
---|
[477] | 171 | phxu->dfdx_cond ( _mu, zeros ( dimu ), C, true ); |
---|
[37] | 172 | // phxu->dfdu_cond ( *_mu,zeros ( dimu ),D,true ); |
---|
| 173 | D.clear(); |
---|
| 174 | |
---|
| 175 | R = R0; |
---|
| 176 | Q = Q0; |
---|
| 177 | |
---|
| 178 | // Cholesky special! |
---|
| 179 | preA.clear(); |
---|
[477] | 180 | preA.set_submatrix ( 0, 0, R._Ch() ); |
---|
| 181 | preA.set_submatrix ( dimy + dimx, dimy, Q._Ch() ); |
---|
[37] | 182 | } |
---|
| 183 | |
---|
| 184 | |
---|
| 185 | void EKFCh::bayes ( const vec &dt ) { |
---|
| 186 | |
---|
[477] | 187 | vec pom ( dimy ); |
---|
| 188 | vec u = dt.get ( dimy, dimy + dimu - 1 ); |
---|
| 189 | vec y = dt.get ( 0, dimy - 1 ); |
---|
[583] | 190 | vec &_mu = est->_mu(); |
---|
| 191 | chmat &_P = est->_R(); |
---|
| 192 | chmat &_Ry = fy._R(); |
---|
| 193 | vec &_yp = fy._mu(); |
---|
| 194 | |
---|
[477] | 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() |
---|
[477] | 199 | preA.set_submatrix ( dimy, 0, ( _P._Ch() ) *C.T() ); |
---|
[37] | 200 | //Fixme can be more efficient if .T() is not used |
---|
[477] | 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 | |
---|
[477] | 207 | if ( !qr ( preA, postA ) ) { |
---|
[565] | 208 | bdm_warning ( "QR in EKFCh unstable!" ); |
---|
[477] | 209 | } |
---|
[37] | 210 | |
---|
[225] | 211 | |
---|
[477] | 212 | ( _Ry._Ch() ) = postA ( 0, dimy - 1, 0, dimy - 1 ); |
---|
| 213 | _K = inv ( A ) * ( postA ( 0, dimy - 1 , dimy, dimy + dimx - 1 ) ).T(); |
---|
| 214 | ( _P._Ch() ) = postA ( dimy, dimy + dimx - 1, dimy, dimy + dimx - 1 ); |
---|
| 215 | |
---|
[62] | 216 | // mat iRY = inv(_Ry->to_mat()); |
---|
| 217 | // mat Stt = Sttm - Sttm * C.T() * iRY * C * Sttm; |
---|
| 218 | // mat _K2 = Stt*C.T()*inv(R.to_mat()); |
---|
| 219 | |
---|
| 220 | // prediction |
---|
[477] | 221 | _mu = pfxu->eval ( _mu , u ); |
---|
| 222 | _yp = phxu->eval ( _mu, u ); |
---|
| 223 | |
---|
| 224 | /* vec mu2 = *_mu + ( _K2 ) * ( y-*_yp );*/ |
---|
| 225 | |
---|
[62] | 226 | //correction //= initial value is already prediction! |
---|
[477] | 227 | backward_substitution ( _Ry._Ch(), ( y - _yp ), pom ); |
---|
| 228 | _mu += ( _K ) * pom ; |
---|
[62] | 229 | |
---|
[477] | 230 | /* _K = (_P->to_mat())*C.transpose() * ( _iRy->to_mat() ); |
---|
| 231 | *_mu = pfxu->eval ( *_mu ,u ) + ( _K )* ( y-*_yp );*/ |
---|
| 232 | |
---|
[215] | 233 | // cout << "P:" <<_P.to_mat() <<endl; |
---|
| 234 | // cout << "Ry:" <<_Ry.to_mat() <<endl; |
---|
[225] | 235 | // cout << "_mu:" <<_mu <<endl; |
---|
| 236 | // cout << "dt:" <<dt <<endl; |
---|
[37] | 237 | |
---|
[477] | 238 | if ( evalll == true ) { //likelihood of observation y |
---|
| 239 | ll = fy.evallog ( y ); |
---|
[37] | 240 | } |
---|
| 241 | } |
---|
| 242 | |
---|
[477] | 243 | void EKFCh::from_setting ( const Setting &set ) { |
---|
[527] | 244 | shared_ptr<diffbifn> IM = UI::build<diffbifn> ( set, "IM", UI::compulsory ); |
---|
| 245 | shared_ptr<diffbifn> OM = UI::build<diffbifn> ( set, "OM", UI::compulsory ); |
---|
[477] | 246 | |
---|
[357] | 247 | //statistics |
---|
[477] | 248 | int dim = IM->dimension(); |
---|
[357] | 249 | vec mu0; |
---|
[477] | 250 | if ( !UI::get ( mu0, set, "mu0" ) ) |
---|
| 251 | mu0 = zeros ( dim ); |
---|
[357] | 252 | |
---|
| 253 | mat P0; |
---|
[471] | 254 | vec dP0; |
---|
[477] | 255 | if ( UI::get ( dP0, set, "dP0" ) ) |
---|
| 256 | P0 = diag ( dP0 ); |
---|
| 257 | else if ( !UI::get ( P0, set, "P0" ) ) |
---|
| 258 | P0 = eye ( dim ); |
---|
| 259 | |
---|
| 260 | set_statistics ( mu0, P0 ); |
---|
| 261 | |
---|
[357] | 262 | //parameters |
---|
[477] | 263 | vec dQ, dR; |
---|
| 264 | UI::get ( dQ, set, "dQ", UI::compulsory ); |
---|
| 265 | UI::get ( dR, set, "dR", UI::compulsory ); |
---|
| 266 | set_parameters ( IM, OM, diag ( dQ ), diag ( dR ) ); |
---|
[357] | 267 | |
---|
| 268 | //connect |
---|
[527] | 269 | shared_ptr<RV> drv = UI::build<RV> ( set, "drv", UI::compulsory ); |
---|
[477] | 270 | set_drv ( *drv ); |
---|
[527] | 271 | shared_ptr<RV> rv = UI::build<RV> ( set, "rv", UI::compulsory ); |
---|
[477] | 272 | set_rv ( *rv ); |
---|
| 273 | |
---|
[357] | 274 | string options; |
---|
[477] | 275 | if ( UI::get ( options, set, "options" ) ) |
---|
| 276 | set_options ( options ); |
---|
[262] | 277 | } |
---|
[357] | 278 | |
---|
[477] | 279 | void MultiModel::from_setting ( const Setting &set ) { |
---|
[357] | 280 | Array<EKFCh*> A; |
---|
[477] | 281 | UI::get ( A, set, "models", UI::compulsory ); |
---|
| 282 | |
---|
| 283 | set_parameters ( A ); |
---|
| 284 | set_drv ( A ( 0 )->_drv() ); |
---|
[357] | 285 | //set_rv(A(0)->_rv()); |
---|
| 286 | |
---|
| 287 | string options; |
---|
[477] | 288 | if ( set.lookupValue ( "options", options ) ) |
---|
| 289 | set_options ( options ); |
---|
[357] | 290 | } |
---|
| 291 | |
---|
| 292 | } |
---|