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