Changeset 703

Show
Ignore:
Timestamp:
11/04/09 22:55:17 (14 years ago)
Author:
smidl
Message:

New transformation between StateSpace? and ARX

Location:
library
Files:
2 modified

Legend:

Unmodified
Added
Removed
  • library/bdm/estim/kalman.h

    r686 r703  
    1919#include "../math/chmat.h" 
    2020#include "../base/user_info.h" 
     21//#include <../applications/pmsm/simulator_zdenek/ekf_example/pmsm_mod.h> 
    2122 
    2223namespace bdm 
     
    500501 
    501502                        A.set_row(0,A1row); 
    502                         C.set_row(0,C1row+D1row*A1row); 
     503                        C.set_row(0,C1row+D1row(0)*A1row); 
    503504                        D.set_row(0,D1row); 
     505                         
     506                } 
     507}; 
     508/*! 
     509State-Space representation of multivariate autoregressive model. 
     510The original model: 
     511\f[ y_t = \theta [\ldots y_{t-k}, \ldots u_{t-l}, \ldots z_{t-m}]' + \Sigma^{-1/2} e_t \f]  
     512where \f$ k,l,m \f$ are maximum delayes of corresponding variables in the regressor. 
     513 
     514The transformed state is: 
     515\f[ x_t = [y_{t} \ldots y_{t-k-1}, u_{t} \ldots u_{t-l-1}, z_{t} \ldots z_{t-m-1}]\f] 
     516 
     517The state accumulates all delayed values starting from time \f$ t \f$ . 
     518 
     519 
     520*/ 
     521class StateFromARX: public StateSpace<fsqmat>{ 
     522        protected: 
     523                //! remember connection from theta ->A 
     524                datalink_part th2A; 
     525                //! xrv 
     526                RV xrv; 
     527                //!function adds n diagonal elements from given starting point r,c 
     528                void diagonal_part(mat &A, int r, int c, int n){ 
     529                        for(int i=0; i<n; i++){A(r,c)=1.0; r++;c++;} 
     530                }; 
     531        public: 
     532                //! set up this object to match given mlnorm 
     533                void connect_mlnorm(const mlnorm<fsqmat> &ml){ 
     534                        //get ids of yrv                                 
     535                        const RV &yrv = ml._rv(); 
     536                        //need to determine u_t - it is all in _rvc that is not in ml._rv() 
     537                        const RV &rgr = ml._rvc(); 
     538                        RV rgr0 = rgr.remove_time(); 
     539                        RV urv = rgr0.subt(yrv);  
     540                                                 
     541                        // create names for state variables 
     542                        xrv=yrv;  
     543                         
     544                        int y_multiplicity = rgr.mint(yrv); 
     545                        int y_block_size = yrv.length()*(y_multiplicity+1); // current yt + all delayed yts 
     546                        for (int m=0;m<y_multiplicity;m++){  
     547                                xrv.add(yrv.copy_t(-m-1)); //add delayed yt 
     548                        } 
     549                        // add regressors 
     550                        ivec u_block_sizes(urv.length()); // no_blocks = yt + unique rgr 
     551                        for (int r=0;r<urv.length();r++){ 
     552                                RV R=urv.subselect(vec_1(r)); //non-delayed element of rgr 
     553                                int r_size=urv.size(r); 
     554                                int r_multiplicity=rgr.mint(R); 
     555                                u_block_sizes(r)= r_size*r_multiplicity ; 
     556                                for(int m=0;m<r_multiplicity;m++){  
     557                                        xrv.add(R.copy_t(-m-1)); //add delayed yt 
     558                                } 
     559                        } 
     560                         
     561                        // get mapp 
     562                        th2A.set_connection(xrv, ml._rvc()); 
     563                         
     564                        //set matrix sizes 
     565                        this->A=zeros(xrv._dsize(),xrv._dsize()); 
     566                        //create y block 
     567                        diagonal_part(this->A, yrv._dsize(), 0, y_block_size-yrv._dsize() ); 
     568                         
     569                        this->B=zeros(xrv._dsize(), urv._dsize()); 
     570                        //add diagonals for rgr 
     571                        int active_x=y_block_size; 
     572                        for (int r=0; r<urv.length(); r++){ 
     573                                diagonal_part( this->A, active_x+urv.size(r),active_x, u_block_sizes(r)-urv.size(r)); 
     574                                this->B.set_submatrix(active_x, 0, eye(urv.size(r))); 
     575                                active_x+=u_block_sizes(r); 
     576                        } 
     577                        this->C=zeros(yrv._dsize(), xrv._dsize()); 
     578                        this->C.set_submatrix(0,0,eye(yrv._dsize())); 
     579                                this->D=zeros(yrv._dsize(),urv._dsize()); 
     580                                this->R = zeros(yrv._dsize(),yrv._dsize()); 
     581                                this->Q = zeros(xrv._dsize(),xrv._dsize()); 
     582                                // Q is set by update 
     583                                                                 
     584                                update_from(ml); 
     585                                validate(); 
     586                }; 
     587                //! fast function to update parameters from ml - not checked for compatibility!! 
     588                void update_from(const mlnorm<fsqmat> &ml){ 
     589                         
     590                        vec theta = ml._A().get_row(0); // this  
     591                        this->Q._M().set_submatrix(0,0,ml._R()); 
    504592                         
    505593                } 
  • library/tests/LQG_test.cpp

    r699 r703  
    3939} 
    4040 
     41TEST(to_state_arx_test) { 
     42        mlnorm<fsqmat> ml; 
     43        mat A="1.1, 2.3"; 
     44        ml.set_parameters(A, vec_1(1.3), eye(1)); 
     45        RV yr=RV("y",1); 
     46        RV ur=RV("u",1); 
     47        ml.set_rv(yr); 
     48        yr.t_plus(-1); 
     49        ml.set_rvc(concat(yr, ur)); 
     50         
     51        shared_ptr<StateFromARX> Stsp=new StateFromARX; 
     52        Stsp->connect_mlnorm(ml); 
     53         
     54        /* results from  
     55        [A,B,C,D]=tf2ss([2.3 0],[1 -1.1]) 
     56        */ 
     57        cout << "---" << endl; 
     58} 
     59 
    4160TEST(arx_LQG_test){ 
    4261        mlnorm<fsqmat> ml; 
    4362        mat A="1.81, -.8189, .00468, .00438"; 
    44         ml.set_parameters(A, vec_1(0), eye(0.0001)); 
     63        ml.set_parameters(A, vec_1(0.0), 0.00001*eye(1)); 
    4564        RV yr=RV("y",1); 
    4665        RV ur=RV("u",1); 
     
    4867        rgr.add(yr.copy_t(-2)); 
    4968        rgr.add(yr.copy_t(-2)); 
     69        rgr.add(ur.copy_t(-2)); 
    5070        rgr.add(ur.copy_t(-1)); 
    51         rgr.add(ur); 
    5271         
    5372        ml.set_rv(yr); 
     
    6483         
    6584        L.redesign(); 
     85        cout << L.to_string()<<endl; 
    6686}