Changeset 739 for library/bdm/estim/kalman.cpp

Timestamp:

11/25/09 18:02:21 (14 years ago)

Author:

mido

Message:

the rest of h to cpp movements, with exception of from_setting and validate to avoid conflicts with Sarka

Files:

• library/bdm/estim/kalman.cpp (modified) (1 diff)

library/bdm/estim/kalman.cpp

@@ -150 +150 @@
 }
 
+void StateCanonical::connect_mlnorm ( const mlnorm<fsqmat> &ml ) {
+        //get ids of yrv
+        const RV &yrv = ml._rv();
+        //need to determine u_t - it is all in _rvc that is not in ml._rv()
+        RV rgr0 = ml._rvc().remove_time();
+        RV urv = rgr0.subt ( yrv );
+
+        //We can do only 1d now... :(
+        bdm_assert ( yrv._dsize() == 1, "Only for SISO so far..." );
+
+        // create names for
+        RV xrv; //empty
+        RV Crv; //empty
+        int td = ml._rvc().mint();
+        // assuming strictly proper function!!!
+        for ( int t = -1; t >= td; t-- ) {
+                xrv.add ( yrv.copy_t ( t ) );
+                Crv.add ( urv.copy_t ( t ) );
+        }
+
+        // get mapp
+        th2A.set_connection ( xrv, ml._rvc() );
+        th2C.set_connection ( Crv, ml._rvc() );
+        th2D.set_connection ( urv, ml._rvc() );
+
+        //set matrix sizes
+        this->A = zeros ( xrv._dsize(), xrv._dsize() );
+        for ( int j = 1; j < xrv._dsize(); j++ ) {
+                A ( j, j - 1 ) = 1.0;    // off diagonal
+        }
+        this->B = zeros ( xrv._dsize(), 1 );
+        this->B ( 0 ) = 1.0;
+        this->C = zeros ( 1, xrv._dsize() );
+        this->D = zeros ( 1, urv._dsize() );
+        this->Q = zeros ( xrv._dsize(), xrv._dsize() );
+        // R is set by update
+
+        //set cache
+        this->A1row = zeros ( xrv._dsize() );
+        this->C1row = zeros ( xrv._dsize() );
+        this->D1row = zeros ( urv._dsize() );
+
+        update_from ( ml );
+        validate();
+};
+
+void StateCanonical::update_from ( const mlnorm<fsqmat> &ml ) {
+
+        vec theta = ml._A().get_row ( 0 ); // this
+
+        th2A.filldown ( theta, A1row );
+        th2C.filldown ( theta, C1row );
+        th2D.filldown ( theta, D1row );
+
+        R = ml._R();
+
+        A.set_row ( 0, A1row );
+        C.set_row ( 0, C1row + D1row ( 0 ) *A1row );
+        D.set_row ( 0, D1row );
+}
+
+void StateFromARX::connect_mlnorm ( const mlnorm<chmat> &ml, RV &xrv, RV &urv ) {
+        //get ids of yrv
+        const RV &yrv = ml._rv();
+        //need to determine u_t - it is all in _rvc that is not in ml._rv()
+        const RV &rgr = ml._rvc();
+        RV rgr0 = rgr.remove_time();
+        urv = rgr0.subt ( yrv );
+
+        // create names for state variables
+        xrv = yrv;
+
+        int y_multiplicity = -rgr.mint ( yrv );
+        int y_block_size = yrv.length() * ( y_multiplicity ); // current yt + all delayed yts
+        for ( int m = 0; m < y_multiplicity - 1; m++ ) { // ========= -1 is important see arx2statespace_notes
+                xrv.add ( yrv.copy_t ( -m - 1 ) ); //add delayed yt
+        }
+        //! temporary RV for connection to ml.rvc, since notation of xrv and ml.rvc does not match
+        RV xrv_ml = xrv.copy_t ( -1 );
+
+        // add regressors
+        ivec u_block_sizes ( urv.length() ); // no_blocks = yt + unique rgr
+        for ( int r = 0; r < urv.length(); r++ ) {
+                RV R = urv.subselect ( vec_1 ( r ) ); //non-delayed element of rgr
+                int r_size = urv.size ( r );
+                int r_multiplicity = -rgr.mint ( R );
+                u_block_sizes ( r ) = r_size * r_multiplicity ;
+                for ( int m = 0; m < r_multiplicity; m++ ) {
+                        xrv.add ( R.copy_t ( -m - 1 ) ); //add delayed yt
+                        xrv_ml.add ( R.copy_t ( -m - 1 ) ); //add delayed yt
+                }
+        }
+        // add constant
+        if ( any ( ml._mu_const() != 0.0 ) ) {
+                have_constant = true;
+                xrv.add ( RV ( "bdm_reserved_constant_one", 1 ) );
+        } else {
+                have_constant = false;
+        }
+
+
+        // get mapp
+        th2A.set_connection ( xrv_ml, ml._rvc() );
+        th2B.set_connection ( urv, ml._rvc() );
+
+        //set matrix sizes
+        this->A = zeros ( xrv._dsize(), xrv._dsize() );
+        //create y block
+        diagonal_part ( this->A, yrv._dsize(), 0, y_block_size - yrv._dsize() );
+
+        this->B = zeros ( xrv._dsize(), urv._dsize() );
+        //add diagonals for rgr
+        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 ) ) );
+                active_x += u_block_sizes ( r );
+        }
+        this->C = zeros ( yrv._dsize(), xrv._dsize() );
+        this->C.set_submatrix ( 0, 0, eye ( yrv._dsize() ) );
+        this->D = zeros ( yrv._dsize(), urv._dsize() );
+        this->R.setCh ( zeros ( yrv._dsize(), yrv._dsize() ) );
+        this->Q.setCh ( zeros ( xrv._dsize(), xrv._dsize() ) );
+        // Q is set by update
+
+        update_from ( ml );
+        validate();
+}
+        
+void StateFromARX::update_from ( const mlnorm<chmat> &ml ) {
+        vec Arow = zeros ( A.cols() );
+        vec Brow = zeros ( B.cols() );
+        //  ROW- WISE EVALUATION =====
+        for ( int i = 0; i < ml._rv()._dsize(); i++ ) {
+
+                vec theta = ml._A().get_row ( i );
+
+                th2A.filldown ( theta, Arow );
+                if ( have_constant ) {
+                        // constant is always at the end no need for datalink
+                        Arow ( A.cols() - 1 ) = ml._mu_const() ( i );
+                }
+                this->A.set_row ( i, Arow );
+
+                th2B.filldown ( theta, Brow );
+                this->B.set_row ( i, Brow );
+        }
+        this->Q._Ch().set_submatrix ( 0, 0, ml.__R()._Ch() );
+
+}