Changeset 586 for library/bdm/estim/kalman.h

Timestamp:

08/27/09 15:39:35 (15 years ago)

Author:

smidl

Message:

redesign of ctrl LQ control for arx

Files:

• library/bdm/estim/kalman.h (modified) (5 diffs)

library/bdm/estim/kalman.h

@@ -78 +78 @@
                         validate();
                 }               
+                //! access function
+                int _dimx(){return dimx;}
+                //! access function
+                int _dimy(){return dimy;}
+                //! access function
+                int _dimu(){return dimu;}
+                //! access function
+                mat _A(){return A;}
+                //! access function
+                mat _B(){return B;}
+                //! access function
+                mat _C(){return C;}
+                //! access function
+                mat _D(){return D;}
+                //! access function
+                sq_T _Q(){return Q;}
+                //! access function
+                sq_T _R(){return R;}
 };
 
@@ -135 +153 @@
                 void bayes (const vec &dt);
 };
-
+UIREGISTER(KalmanFull);
 
 
@@ -182 +200 @@
                 void bayes (const vec &dt);
                 
-                void from_settings(const Setting &set){
+                void from_setting(const Setting &set){
                         Kalman<chmat>::from_setting(set);
                         initialize();
                 }
 };
+UIREGISTER(KalmanCh);
 
 /*!
@@ -219 +238 @@
                 }
 };
+UIREGISTER(EKFfull);
+
 
 /*!
@@ -330 +351 @@
 SHAREDPTR (MultiModel);
 
+//! conversion of outer ARX model (mlnorm) to state space model 
+/*!
+The model is constructed as:
+\f[ x_{t+1} = Ax_t + B u_t + R^{1/2} e_t, y_t=Cx_t+Du_t + R^{1/2}w_t, \f]
+For example, for:
+\f[ y_t = a y_{t-1} + b u_{t-1}\f]
+the state \f$ x_t = [y_{t-1}, u_{t-1}] \f$
+*/
+template<class sq_T>
+shared_ptr<StateSpace<fsqmat> > to_state_space(const mlnorm<sq_T > &ml, ivec &theta_in_A, ivec &theta_in_C){
+        //get ids of yrv
+                        
+                        ivec yids= unique(ml._rv()._ids()); //yrv is now stored in _yrv
+                        ivec dids= unique(ml._rvc()._ids()); //yrv is now stored in _yrv
+                        ivec uids=unique_complement(dids, yids);
+        
+                        RV yrv_uni = RV(yids,zeros_i(yids.length()));
+                        RV urv_uni = RV(uids,zeros_i(yids.length()));
+                        //We can do only 1d now... :(
+                        bdm_assert_debug(yrv_uni._dsize()==urv_uni._dsize()==1, "Only for SISO so far..." );
+                                        
+                        RV xrv; //empty
+                        RV Crv; //empty
+                        int td=ml._rvc().mintd();
+                        for (int t=-1;t>=td;t--){
+                                xrv.add(yrv_uni);
+                                Crv.add(urv_uni);
+                        }
+                        
+                        int dimx = xrv._dsize();
+                        
+                        theta_in_A = ml._rv().dataind(xrv);
+                        theta_in_C = ml._rvc().dataind(xrv);
+                        // some chcek of corretness
+                        
+                        vec A1row = zeros(xrv._dsize());
+                        vec C1row = zeros(xrv._dsize());
+                        vec theta = ml._A().get_row(0); // this 
+                        set_subvector( A1row, theta_in_A, theta);
+                        set_subvector( C1row, theta_in_C, theta);
+
+                        StateSpace<fsqmat> stsp=new StateSpace<fsqmat>();
+                        mat A=zeros(dimx,dimx);
+                        A.set_row(0,A1row);
+                        for (int j=1; j<dimx; j++){A(j,j-1)=1.0;} // off diagonal
+                        mat B=zeros(dimx,1);
+                        B(0) = 1.0;
+                        mat C=zeros(1,dimx);
+                        C.set_row(0,C1row);
+                        stsp.set_parameters(A,B,C,zeros(1,1), zeros(dimx,dimx), ml._R());
+                        return stsp;
+}
+
 /////////// INSTANTIATION