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