Changeset 653

Timestamp:

10/12/09 19:38:54 (15 years ago)

Author:

smidl

Message:

corrections in Kalman and particles

Files:

• applications/bdmtoolbox/CMakeLists.txt (modified) (1 diff)
• applications/bdmtoolbox/mex/estimator.cpp (modified) (1 diff)
• library/bdm/estim/kalman.cpp (modified) (4 diffs)
• library/bdm/estim/kalman.h (modified) (6 diffs)
• library/bdm/estim/particles.h (modified) (2 diffs)

applications/bdmtoolbox/CMakeLists.txt

@@ -12 +12 @@
 
 include_directories (${BDM_SOURCE_DIR}/bdm)
-link_directories (${BDM_BINARY_DIR}/bdm)
+link_directories (${BDM_SOURCE_DIR}/bdm)
 
 #EXEC(estimator)

applications/bdmtoolbox/mex/estimator.cpp

@@ -204 +204 @@
         }
 #endif
+        for (int i=0;i<Dls_buf.length(); i++){delete Dls_buf(i);}
+        for (int i=0;i<Dls.length(); i++){delete Dls(i);}
 }

library/bdm/estim/kalman.cpp

@@ -16 +16 @@
         mat &P = est->_R();
         mat& _Ry = fy._R();
+        vec& yp = fy._mu();
         //Time update
         mu = A * mu + B * u;
@@ -24 +25 @@
         _K = P * C.transpose() * inv ( _Ry );
         P -= _K * C * P; // P = P -KCP;
-        mu += _K * ( y - C * mu - D * u );
+        yp = C * mu + D * u;
+        mu += _K * ( y - yp );
 
         if ( evalll ) {
-                ll=est->evallog(y);
+                ll=fy.evallog(y);
         }
 };
@@ -65 +67 @@
         mat &P = est->_R();
         mat& _Ry = fy._R();
+        vec& yp = fy._mu();
         
         pfxu->dfdx_cond ( mu, zeros ( dimu ), A, true );
@@ -77 +80 @@
         _K = P * C.transpose() * inv ( _Ry );
         P -= _K * C * P; // P = P -KCP;
-        vec yp = phxu->eval ( mu, u );
+        yp = phxu->eval ( mu, u );
         mu += _K * ( y - yp );
 

library/bdm/estim/kalman.h

@@ -54 +54 @@
         public:
                 StateSpace() : dimx (0), dimy (0), dimu (0), A(), B(), C(), D(), Q(), R() {}
+                StateSpace(const StateSpace<sq_T> &S0) : dimx (S0.dimx), dimy (S0.dimy), dimu (S0.dimu), A(S0.A), B(S0.B), C(S0.C), D(S0.D), Q(S0.Q), R(S0.R) {}
                 void set_parameters (const mat &A0, const  mat &B0, const  mat &C0, const  mat &D0, const  sq_T &Q0, const sq_T &R0);
                 void validate();
@@ -114 +115 @@
                 enorm<sq_T>  fy;
         public:
-                Kalman() : BM(), StateSpace<sq_T>(), yrv(),urv(), _K(),  est(new enorm<sq_T>){}
+                Kalman<sq_T>() : BM(), StateSpace<sq_T>(), yrv(),urv(), _K(),  est(new enorm<sq_T>){}
+                Kalman<sq_T>(const Kalman<sq_T> &K0) : BM(K0), StateSpace<sq_T>(K0), yrv(K0.yrv),urv(K0.urv), _K(K0._K),  est(new enorm<sq_T>(*K0.est)), fy(K0.fy){}
                 void set_statistics (const vec &mu0, const mat &P0) {est->set_parameters (mu0, P0); };
                 void set_statistics (const vec &mu0, const sq_T &P0) {est->set_parameters (mu0, P0); };
@@ -152 +154 @@
                 //! Here dt = [yt;ut] of appropriate dimensions
                 void bayes (const vec &dt);
+                BM* _copy_() const {
+                        KalmanFull* K = new KalmanFull;
+                        K->set_parameters (A, B, C, D, Q, R);
+                        K->set_statistics (est->_mu(), est->_R());
+                        return K;
+                }
 };
 UIREGISTER(KalmanFull);
@@ -236 +244 @@
                 const mat _R() {
                         return est->_R().to_mat();
+                }
+                void from_setting (const Setting &set) {
+                        shared_ptr<diffbifn> IM = UI::build<diffbifn> ( set, "IM", UI::compulsory );
+                        shared_ptr<diffbifn> OM = UI::build<diffbifn> ( set, "OM", UI::compulsory );
+                        
+                        //statistics
+                        int dim = IM->dimension();
+                        vec mu0;
+                        if ( !UI::get ( mu0, set, "mu0" ) )
+                                mu0 = zeros ( dim );
+                        
+                        mat P0;
+                        vec dP0;
+                        if ( UI::get ( dP0, set, "dP0" ) )
+                                P0 = diag ( dP0 );
+                        else if ( !UI::get ( P0, set, "P0" ) )
+                                P0 = eye ( dim );
+                        
+                        set_statistics ( mu0, P0 );
+                        
+                        //parameters
+                        vec dQ, dR;
+                        UI::get ( dQ, set, "dQ", UI::compulsory );
+                        UI::get ( dR, set, "dR", UI::compulsory );
+                        set_parameters ( IM, OM, diag ( dQ ), diag ( dR ) );
+                        
+                        //connect
+                        shared_ptr<RV> drv = UI::build<RV> ( set, "drv", UI::compulsory );
+                        set_drv ( *drv );
+                        shared_ptr<RV> rv = UI::build<RV> ( set, "rv", UI::compulsory );
+                        set_rv ( *rv );
+                        
+                        string options;
+                        if ( UI::get ( options, set, "options" ) )
+                                set_options ( options );
+//                      pfxu = UI::build<diffbifn>(set, "IM", UI::compulsory);
+//                      phxu = UI::build<diffbifn>(set, "OM", UI::compulsory);
+//                      
+//                      mat R0;
+//                      UI::get(R0, set, "R",UI::compulsory);
+//                      mat Q0;
+//                      UI::get(Q0, set, "Q",UI::compulsory);
+//                      
+//                      
+//                      mat P0; vec mu0;
+//                      UI::get(mu0, set, "mu0", UI::optional);
+//                      UI::get(P0, set,  "P0", UI::optional);
+//                      set_statistics(mu0,P0);
+//                      // Initial values
+//                      UI::get (yrv, set, "yrv", UI::optional);
+//                      UI::get (urv, set, "urv", UI::optional);
+//                      set_drv(concat(yrv,urv));
+// 
+//                      // setup StateSpace
+//                      pfxu->dfdu_cond(mu0, zeros(pfxu->_dimu()), A,true);
+//                      phxu->dfdu_cond(mu0, zeros(pfxu->_dimu()), C,true);
+//                      
+                        validate();
+                }
+                void validate() {
+                        // check stats and IM and OM
                 }
 };
@@ -448 +517 @@
 void StateSpace<sq_T>::set_parameters (const mat &A0, const  mat &B0, const  mat &C0, const  mat &D0, const  sq_T &Q0, const sq_T &R0)
 {
-        dimx = A0.rows();
-        dimu = B0.cols();
-        dimy = C0.rows();
 
         A = A0;
@@ -463 +529 @@
 template<class sq_T>
 void StateSpace<sq_T>::validate(){
+        dimx = A.rows();
+        dimu = B.cols();
+        dimy = C.rows();
         bdm_assert (A.cols() == dimx, "KalmanFull: A is not square");
         bdm_assert (B.rows() == dimx, "KalmanFull: B is not compatible");
         bdm_assert (C.cols() == dimx, "KalmanFull: C is not square");
-        bdm_assert ( (D.rows() == dimy) || (D.cols() == dimu), "KalmanFull: D is not compatible");
-        bdm_assert ( (Q.cols() == dimx) || (Q.rows() == dimx), "KalmanFull: Q is not compatible");
-        bdm_assert ( (R.cols() == dimy) || (R.rows() == dimy), "KalmanFull: R is not compatible");
+        bdm_assert ( (D.rows() == dimy) && (D.cols() == dimu), "KalmanFull: D is not compatible");
+        bdm_assert ( (Q.cols() == dimx) && (Q.rows() == dimx), "KalmanFull: Q is not compatible");
+        bdm_assert ( (R.cols() == dimy) && (R.rows() == dimy), "KalmanFull: R is not compatible");
 }
 

library/bdm/estim/particles.h

@@ -190 +190 @@
                 est.resample(ind,resmethod);
         }
+        Array<vec>& __samples(){return _samples;}
 };
 UIREGISTER(PF);
@@ -201 +202 @@
 
 class MPF : public BM  {
-    shared_ptr<PF> pf;
+        protected:
+        shared_ptr<PF> pf;
         Array<BM*> BMs;