Changeset 744 for library/bdm/estim

Timestamp:

11/28/09 14:38:48 (15 years ago)

Author:

smidl

Message:

Working unitsteps and controlloop + corresponding fixes

Location:

library/bdm/estim

Files:

• ekf_template.h (modified) (3 diffs)
• kalman.cpp (modified) (1 diff)
• kalman.h (modified) (1 diff)
• particles.h (modified) (2 diffs)

library/bdm/estim/ekf_template.h

@@ -22 +22 @@
 public:
         void condition ( const vec &QR0 ) {
-                Q = diag ( QR0 ( 0, dimx - 1 ) );
-                R = diag ( QR0 ( dimx, dimx + dimy - 1 ) );
+                Q = diag ( QR0 ( 0, dimension() - 1 ) );
+                R = diag ( QR0 ( dimension(), dimension() + dimy - 1 ) );
         };
 };
@@ -30 +30 @@
 class EKFCh_dQ : public EKFCh {
 public:
-        void condition ( const vec &Q0 ) {
-                Q.setD ( Q0, 0 );
+        BM* _copy_() const {return new EKFCh_dQ(*this);}
+        //! new bayes, expects cond = [ut, Qt]
+        void bayes( const vec &yt , const vec &cond) {
+                vec ut=cond.left(dimc-dimension());
+                vec dQt=cond.right(dimension());
+                Q.setD ( dQt, 0 );
                 //from EKF
-                preA.set_submatrix ( dimy + dimx, dimy, Q._Ch() );
+                preA.set_submatrix ( dimy + dimension(), dimy, Q._Ch() );
+                
+                EKFCh::bayes(yt,ut);
         };
+        void validate() {
+                EKFCh::validate();
+                dimc += dimension();
+        }
 };
 
@@ -43 +53 @@
                 Q.setCh ( chQ0 );
                 //from EKF
-                preA.set_submatrix ( dimy + dimx, dimy, Q._Ch() );
+                preA.set_submatrix ( dimy + dimension(), dimension(), Q._Ch() );
         };
 };

library/bdm/estim/kalman.cpp

@@ -264 +264 @@
         int active_x = y_block_size;
         for ( int r = 0; r < urv.length(); r++ ) {
-                diagonal_part ( this->A, active_x + urv.size ( r ), active_x, u_block_sizes ( r ) - urv.size ( r ) );
-                this->B.set_submatrix ( active_x, 0, eye ( urv.size ( r ) ) );
+                if (u_block_sizes(r)>0){
+                        diagonal_part ( this->A, active_x + urv.size ( r ), active_x, u_block_sizes ( r ) - urv.size ( r ) );
+                        this->B.set_submatrix ( active_x, 0, eye ( urv.size ( r ) ) );
+                }
                 active_x += u_block_sizes ( r );
         }

library/bdm/estim/kalman.h

@@ -336 +336 @@
         //! copy constructor duplicated - calls different set_parameters
         BM* _copy_() const {
-                EKFCh* E = new EKFCh;
-                E->set_parameters ( pfxu, phxu, Q, R );
-                E->set_statistics ( est._mu(), est._R() );
-                return E;
+                return new EKFCh(*this);
         }
         //! Set nonlinear functions for mean values and covariance matrices.

library/bdm/estim/particles.h

@@ -267 +267 @@
         MPF () :  jest ( pf, BMs ) {};
         //! set all parameters at once
-        void set_parameters ( shared_ptr<pdf> par0, shared_ptr<pdf> obs0, int n0, RESAMPLING_METHOD rm = SYSTEMATIC ) {
-                pf->set_model ( par0, obs0 );
+        void set_pf ( shared_ptr<pdf> par0, int n0, RESAMPLING_METHOD rm = SYSTEMATIC ) {
+                if (!pf) pf=new PF;
+                pf->set_model ( par0, par0 ); // <=== nasty!!!
                 pf->set_parameters ( n0, rm );
+                pf->set_rv(par0->_rv());
                 BMs.set_length ( n0 );
         }
@@ -298 +300 @@
                 return BMs ( i );
         }
+        PF& _pf() {return *pf;}
 
         /*! configuration structure for basic PF