Changeset 744 for library

Timestamp:

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

Author:

smidl

Message:

Working unitsteps and controlloop + corresponding fixes

Location:

library/bdm

Files:

• base/bdmbase.cpp (modified) (1 diff)
• base/bdmbase.h (modified) (1 diff)
• design/arx_ctrl.h (modified) (1 diff)
• design/ctrlbase.cpp (modified) (2 diffs)
• estim/ekf_template.h (modified) (3 diffs)
• estim/kalman.cpp (modified) (1 diff)
• estim/kalman.h (modified) (1 diff)
• estim/particles.h (modified) (2 diffs)

library/bdm/base/bdmbase.cpp

@@ -495 +495 @@
 void datalink_buffered::set_connection ( const RV &rv, const RV &rv_up ) {
         // create link between up and down
-        datalink_part::set_connection ( rv, rv_up.remove_time() ); // only non-delayed version
+        datalink_part::set_connection ( rv, rv_up); // only non-delayed version
 
         // create rvs of history
         // we can store only what we get in rv_up - everything else is removed
-        ivec valid_ids = rv.findself_ids ( rv_up );
-        RV rv_hist = rv.subselect ( find ( valid_ids >= 0 ) ); // select only rvs that are in rv_up
+        ivec valid_ids = rv.findself_ids ( rv_up ); // return on which position each id is
+        RV rv_hist = rv.subselect ( find ( valid_ids >= 0 ) ); // select only rvs that are in rv_up, ie ind>0
         RV rv_hist0 = rv_hist.remove_time(); // these RVs will form history at time =0
         // now we need to know what is needed from Up

library/bdm/base/bdmbase.h

@@ -1096 +1096 @@
                 yrv = rv;
         }
+        //! access function
+        void set_rvc ( const RV &rv ) {
+                rvc = rv;
+        }
         //! access to rv of the posterior
         void set_rv ( const RV &rv ) {

library/bdm/design/arx_ctrl.h

@@ -124 +124 @@
                 lq.validate();
 
+                rv = urvm; // rv is not shifted to t+1!!
                 rvc = concat ( xrv, urvm );
                 rvc2ar_y.set_connection ( ar->_yrv(), rvc );

library/bdm/design/ctrlbase.cpp

@@ -13 +13 @@
         //penalization
         qux.set_submatrix ( 0, 0, Qu._Ch() );
-        qux.set_submatrix ( 0, dimx + dimu + dimy, Qu._Ch() );
+        qux.set_submatrix ( 0, dimx + dimu + dimy, -Qu._Ch() );
 
         qyx.set_submatrix ( 0, 0, S->_C() );
@@ -57 +57 @@
 
 void LQG::ricatti_step() {
-        pre_qr.set_submatrix ( 0, 0, s*pr );
-        pre_qr.set_submatrix ( dimx + dimu + dimy, dimu + dimx, -Qy.to_mat() *y_req );
+//      pre_qr.set_submatrix ( 0, 0, s*pr );
+//      pre_qr.set_submatrix ( dimx + dimu + dimy, dimu + dimx, -Qy.to_mat() *y_req );
         if ( !qr ( pre_qr, post_qr ) ) {
                 bdm_warning ( "QR in LQG unstable" );

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