Legend:
- Unmodified
- Added
- Removed
-
bdm/estim/libKF.cpp
r27 r28 28 28 } 29 29 30 void KalmanFull::bayes( const vec &dt , bool evalll) {30 void KalmanFull::bayes( const vec &dt) { 31 31 it_assert_debug( dt.length()==( dimy+dimu ),"KalmanFull::bayes wrong size of dt" ); 32 32 -
bdm/estim/libKF.h
r22 r28 16 16 #include <itpp/itbase.h> 17 17 #include "../stat/libFN.h" 18 #include "../ math/libDC.h"18 #include "../stat/libEF.h" 19 19 20 20 … … 41 41 KalmanFull ( mat A, mat B, mat C, mat D, mat R, mat Q, mat P0, vec mu0); 42 42 //! Here dt = [yt;ut] of appropriate dimensions 43 void bayes(const vec &dt, bool evalll=true); 43 void bayes(const vec &dt); 44 epdf* _epdf(){return NULL;}; 44 45 45 46 friend std::ostream &operator<< ( std::ostream &os, const KalmanFull &kf ); … … 58 59 sq_T R, Q; 59 60 60 //cache 61 //!posterior on $x_t$ 62 enorm<sq_T> est; 63 //!preditive density on $y_t$ 64 enorm<sq_T> fy; 65 61 66 mat _K; 62 vec _yp; 63 sq_T _Ry,_iRy; 64 public: 65 //posterior 66 //! Mean value of the posterior density 67 vec mu; 68 //! Mean value of the posterior density 69 sq_T P; 70 67 //cache of fy 68 vec* _yp; 69 sq_T* _Ry,*_iRy; 70 //cache of est 71 vec* _mu; 72 sq_T* _P, *_iP; 73 71 74 public: 72 75 //! Default constructor 73 Kalman (int dimx, int dimu, int dimy); 74 //! Full constructor 75 Kalman ( mat A0, mat B0, mat C0, mat D0, sq_T R0, sq_T Q0, sq_T P0, vec mu0 ); 76 Kalman (RV rvx0, RV rvy0, RV rvu0); 77 //! Set parameters with check of relevance 78 void set_parameters (const mat &A0,const mat &B0,const mat &C0,const mat &D0,const sq_T &R0,const sq_T &Q0); 79 //! Set estimate values, used e.g. in initialization. 80 void set_est(const vec &mu0, const sq_T &P0 ){est.set_parameters(mu0,P0);}; 76 81 //! Here dt = [yt;ut] of appropriate dimensions 77 void bayes(const vec &dt, bool evalll=true); 78 79 friend std::ostream &operator<< ( std::ostream &os, const KalmanFull &kf ); 80 82 void bayes(const vec &dt); 83 84 // friend std::ostream &operator<< ( std::ostream &os, const Kalman<sq_T> &kf ); 85 86 //!access functions 87 enorm<sq_T>* _epdf(){return &est;} 81 88 }; 82 89 … … 87 94 */ 88 95 template<class sq_T> 89 class EKF : public Kalman< fsqmat> {96 class EKF : public Kalman<ldmat> { 90 97 //! Internal Model f(x,u) 91 98 diffbifn* pfxu; … … 94 101 public: 95 102 //! Default constructor 96 EKF (diffbifn* pfxu, diffbifn* phxu, sq_T Q0, sq_T R0, vec mu0, mat P0); 103 EKF (RV rvx, RV rvy, RV rvu); 104 void set_parameters(diffbifn* pfxu, diffbifn* phxu, const sq_T Q0, const sq_T R0); 97 105 //! Here dt = [yt;ut] of appropriate dimensions 98 void bayes(const vec &dt , bool evalll=true);106 void bayes(const vec &dt); 99 107 }; 100 108 … … 102 110 103 111 template<class sq_T> 104 Kalman<sq_T>::Kalman( int dx, int du, int dy): BM(), dimx(dx),dimy(dy),dimu(du){ 105 A = mat(dimx,dimx); 106 B = mat(dimx,dimu); 107 C = mat(dimy,dimx); 108 D = mat(dimy,dimu); 109 110 mu = vec(dimx); 111 //TODO Initialize the rest? 112 }; 113 114 template<class sq_T> 115 Kalman<sq_T>::Kalman(const mat A0,const mat B0, const mat C0, const mat D0, const sq_T R0, const sq_T Q0, const sq_T P0, const vec mu0 ): BM() { 116 dimx = A0.rows(); 117 dimu = B0.cols(); 118 dimy = C0.rows(); 119 112 Kalman<sq_T>::Kalman(RV rvx, RV rvy, RV rvu): BM(), 113 dimx(rvx.count()), dimy(rvy.count()),dimu(rvu.count()), 114 A(dimx,dimx), B(dimx,dimu), C(dimy,dimx), D(dimy,dimu), est(rvx), fy(rvy){ 115 //assign cache 116 //est 117 _mu = est._mu(); 118 est._R(_P,_iP); 119 120 //fy 121 _yp = fy._mu(); 122 fy._R(_Ry,_iRy); 123 }; 124 125 template<class sq_T> 126 void Kalman<sq_T>::set_parameters(const mat &A0,const mat &B0, const mat &C0, const mat &D0, const sq_T &R0, const sq_T &Q0) { 120 127 it_assert_debug( A0.cols()==dimx, "Kalman: A is not square" ); 121 128 it_assert_debug( B0.rows()==dimx, "Kalman: B is not compatible" ); … … 130 137 D = D0; 131 138 R = R0; 132 Q = Q0; 133 mu = mu0; 134 P = P0; 135 136 //Fixme should we assign cache?? 137 _iRy = eye(dimy); // needed in inv(_iRy) 138 _Ry = eye(dimy); // needed in inv(_iRy) 139 Q = Q0; 139 140 } 140 141 141 142 template<class sq_T> 142 void Kalman<sq_T>::bayes( const vec &dt , bool evalll) {143 void Kalman<sq_T>::bayes( const vec &dt) { 143 144 it_assert_debug( dt.length()==( dimy+dimu ),"KalmanFull::bayes wrong size of dt" ); 144 145 … … 146 147 vec y = dt.get( 0,dimy-1 ); 147 148 //Time update 148 mu = A*mu+ B*u;149 *_mu = A*(*_mu) + B*u; 149 150 //P = A*P*A.transpose() + Q; in sq_T 150 P.mult_sym( A );151 P+=Q;151 _P->mult_sym( A ); 152 (*_P)+=Q; 152 153 153 154 //Data update 154 155 //_Ry = C*P*C.transpose() + R; in sq_T 155 P.mult_sym( C, _Ry); 156 _Ry+=R; 157 158 mat Pfull = P.to_mat(); 159 160 _Ry.inv( _iRy ); // result is in _iRy; 161 _K = Pfull*C.transpose()*(_iRy.to_mat()); 162 P -= _K*C*Pfull; // P = P -KCP; 163 _yp = y-C*mu-D*u; //y prediction 164 mu += _K*( _yp ); 156 _P->mult_sym( C, *_Ry); 157 (*_Ry)+=R; 158 159 mat Pfull = _P->to_mat(); 160 161 _Ry->inv( *_iRy ); // result is in _iRy; 162 _K = Pfull*C.transpose()*(_iRy->to_mat()); 163 164 sq_T pom((int)Pfull.rows()); 165 _iRy->mult_sym_t(C*Pfull,pom); 166 (*_P) -= pom; // P = P -PC'iRy*CP; 167 (*_yp) = C*(*_mu)+D*u; //y prediction 168 (*_mu) += _K*( y-(*_yp) ); 165 169 166 170 if (evalll==true) { 167 ll+= -0.5*(_Ry.cols()*0.79817986835811504957 \ 168 +_Ry.logdet() +_iRy.qform(_yp)); 171 ll+=fy.evalpdflog(*_yp); 169 172 } 170 173 }; … … 173 176 174 177 template<class sq_T> 175 EKF<sq_T>::EKF( diffbifn* pfxu0, diffbifn* phxu0, sq_T Q0, sq_T R0, vec mu0, mat P0): pfxu(pfxu0), phxu(phxu0), Kalman<fsqmat>(pfxu0->_dimx(),pfxu0->_dimu(),phxu0->_dimy()) { 178 EKF<sq_T>::EKF(RV rvx0, RV rvy0, RV rvu0): Kalman<ldmat>(rvx0,rvy0,rvu0){} 179 180 template<class sq_T> 181 void EKF<sq_T>::set_parameters(diffbifn* pfxu0, diffbifn* phxu0,const sq_T Q0,const sq_T R0) { 182 pfxu = pfxu0; 183 phxu = phxu0; 176 184 177 185 //initialize matrices A C, later, these will be only updated! 178 pfxu->dfdx_cond(mu,zeros(dimu),A,true); 179 pfxu->dfdu_cond(mu,zeros(dimu),B,true); 180 phxu->dfdx_cond(mu,zeros(dimu),C,true); 181 phxu->dfdu_cond(mu,zeros(dimu),D,true); 182 186 pfxu->dfdx_cond(*_mu,zeros(dimu),A,true); 187 pfxu->dfdu_cond(*_mu,zeros(dimu),B,true); 188 phxu->dfdx_cond(*_mu,zeros(dimu),C,true); 189 phxu->dfdu_cond(*_mu,zeros(dimu),D,true); 183 190 184 191 R = R0; 185 192 Q = Q0; 186 mu = mu0;187 P = P0;188 193 189 194 using std::cout; … … 196 201 197 202 template<class sq_T> 198 void EKF<sq_T>::bayes( const vec &dt , bool evalll) {203 void EKF<sq_T>::bayes( const vec &dt) { 199 204 it_assert_debug( dt.length()==( dimy+dimu ),"KalmanFull::bayes wrong size of dt" ); 200 205 … … 202 207 vec y = dt.get( 0,dimy-1 ); 203 208 //Time update 204 mu = pfxu->eval(mu, u);205 pfxu->dfdx_cond( mu,u,A,false); //update A by a derivative of fx209 *_mu = pfxu->eval(*_mu, u); 210 pfxu->dfdx_cond(*_mu,u,A,false); //update A by a derivative of fx 206 211 207 212 //P = A*P*A.transpose() + Q; in sq_T 208 P.mult_sym( A );209 P+=Q;213 _P->mult_sym( A ); 214 (*_P)+=Q; 210 215 211 216 //Data update 212 phxu->dfdx_cond( mu,u,C,false); //update C by a derivative hx217 phxu->dfdx_cond(*_mu,u,C,false); //update C by a derivative hx 213 218 //_Ry = C*P*C.transpose() + R; in sq_T 214 P.mult_sym( C, _Ry); 215 _Ry+=R; 216 217 mat Pfull = P.to_mat(); 218 219 _Ry.inv( _iRy ); // result is in _iRy; 220 _K = Pfull*C.transpose()*(_iRy.to_mat()); 221 P -= _K*C*Pfull; // P = P -KCP; 222 _yp = y-phxu->eval(mu,u); //y prediction 223 mu += _K*( _yp ); 224 225 if (evalll==true) { 226 ll+= -0.5*(_Ry.cols()*0.79817986835811504957 \ 227 +_Ry.logdet() +_iRy.qform(_yp)); 228 } 219 _P->mult_sym( C, *_Ry); 220 (*_Ry)+=R; 221 222 mat Pfull = _P->to_mat(); 223 224 _Ry->inv( *_iRy ); // result is in _iRy; 225 _K = Pfull*C.transpose()*(_iRy->to_mat()); 226 (*_P) -= _K*C*Pfull; // P = P -KCP; 227 *_yp = phxu->eval(*_mu,u); //y prediction 228 (*_mu) += _K*( y-*_yp ); 229 230 if (evalll==true) {ll+=est.evalpdflog(y);} 229 231 }; 230 232 -
bdm/estim/libPF.cpp
r14 r28 81 81 } 82 82 83 TrivialPF::TrivialPF( mpdf &par0, mpdf &obs0, mpdf&prop0, int n0 ) : PF(1/n*ones(n)) {83 TrivialPF::TrivialPF( mpdf &par0, mpdf &obs0, BM &prop0, int n0 ) : PF(1/n*ones(n)) { 84 84 is_proposal = true; 85 85 prop = &prop0; … … 99 99 Sort<double> S; 100 100 ivec ind, iw; 101 /*101 102 102 //generate new samples 103 103 for ( i=0;i<n;i++ ) { 104 prop->evalcond( ptcls( i ), &prop_cond ); 105 ptcls( i ) = prop_cond.sample(); 106 gl = prop_cond.eval( ptcls( i ) ); 104 if(is_proposal) { 105 epdf* prop_epdf; 106 // prop_epdf = prop._epdf(); 107 prop->bayes(dt); 108 ptcls( i ) = prop_epdf->sample(); 109 } 110 // gl = prop_cond.eval( ptcls( i ) ); 107 111 108 obs.evalcond( ptcls( i ), &obs_cond );109 ll = obs_cond.eval( dt );112 // obs.evalcond( ptcls( i ), &obs_cond ); 113 // ll = obs_cond.eval( dt ); 110 114 w( i ) *= ll/gl; 111 115 } … … 113 117 for ( i=0;i<n;i++ ){sum+=w( i );}; 114 118 w( i ) /=sum; //? 115 //119 116 120 ind = resample(); 117 121 iw = S.sort_index( 0,n-1,w ); // the first one in iw is the strongest … … 120 124 ptcls( i ) = ptcls( i ); //potentionally dangerous! 121 125 } 122 */126 123 127 } -
bdm/estim/libPF.h
r15 r28 38 38 PF (vec w); 39 39 //TODO remove or implement bayes()! 40 void bayes(const vec &dt, bool evell){}; 40 void bayes(const vec &dt){}; 41 epdf* _epdf(){return NULL;} 41 42 }; 42 43 … … 51 52 52 53 bool is_proposal; 53 mpdf*prop;54 BM *prop; 54 55 mpdf *par; 55 56 mpdf *obs; 56 57 57 58 public: 58 TrivialPF(mpdf &par, mpdf &obs, mpdf&prop, int n0);59 TrivialPF(mpdf &par, mpdf &obs, BM &prop, int n0); 59 60 TrivialPF(mpdf &par, mpdf &obs, int n0); 60 61 void bayes(const vec &dt, bool evalll); … … 64 65 Array<BM> Bms; 65 66 public: 66 MPF(BM &B, mpdf&prop, mpdf &obs, mpdf &par);67 MPF(BM &B, BM &prop, mpdf &obs, mpdf &par); 67 68 void bayes(vec &dt); 68 69 };
