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

Timestamp:

08/17/10 22:06:50 (14 years ago)

Author:

smidl

Message:

Kalman with UD (bierman, thorton) and tests

Files:

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

library/bdm/estim/kalman.h

@@ -362 +362 @@
 
     void validate() {
-        KalmanFull::validate();
+       // KalmanFull::validate();
 
         // check stats and IM and OM
@@ -426 +426 @@
 SHAREDPTR ( EKFCh );
 
+//! EKF using bierman and Thorton code
+class EKF_UD : public BM {
+        protected:
+                //! Internal Model f(x,u)
+                shared_ptr<diffbifn> pfxu;
+                
+                //! Observation Model h(x,u)
+                shared_ptr<diffbifn> phxu;
+                
+                //! U part
+                mat U;
+                //! D part
+                vec D;
+                
+                mat A;
+                mat C;
+                mat Q;
+                vec R;
+
+                enorm<ldmat> est;
+        public:
+                //! copy constructor duplicated 
+                EKF_UD* _copy() const {
+                        return new EKF_UD(*this);
+                }
+
+                const enorm<ldmat>& posterior()const{return est;};
+                
+                enorm<ldmat>& prior() {
+                        return const_cast<enorm<ldmat>&>(posterior());
+                }
+                
+
+                EKF_UD(){}
+                
+                EKF_UD(const EKF_UD &E0): pfxu(E0.pfxu),phxu(E0.phxu), U(E0.U), D(E0.D){}
+                
+                //! Set nonlinear functions for mean values and covariance matrices.
+                void set_parameters ( const shared_ptr<diffbifn> &pfxu, const shared_ptr<diffbifn> &phxu, const mat Q0, const vec R0 );
+                
+                //! Here dt = [yt;ut] of appropriate dimensions
+                void bayes ( const vec &yt, const vec &cond = empty_vec );
+                
+                /*! Create object from the following structure
+                
+                \code
+                class = 'EKF_UD';
+                OM = configuration of bdm::diffbifn;    % any offspring of diffbifn, bdm::diffbifn::from_setting
+                IM = configuration of bdm::diffbifn;    % any offspring of diffbifn, bdm::diffbifn::from_setting
+                dQ = [...];                             % vector containing diagonal of Q
+                dR = [...];                             % vector containing diagonal of R
+                --- optional fields ---
+                mu0 = [...];                            % vector of statistics mu0
+                dP0 = [...];                            % vector containing diagonal of P0
+                -- or --
+                P0 = [...];                             % full matrix P0
+                --- inherited fields ---
+                bdm::BM::from_setting
+                \endcode
+                If the optional fields are not given, they will be filled as follows:
+                \code
+                mu0 = [0,0,0,....];                     % empty statistics
+                P0 = eye( dim );              
+                \endcode
+                */
+                void from_setting ( const Setting &set );
+                
+                void validate() {};
+                // TODO dodelat void to_setting( Setting &set ) const;
+                
+};
+UIREGISTER(EKF_UD);
+
 class UKFCh : public EKFCh{
         public:
@@ -468 +541 @@
         
                 //step 4
-                mat Xpred_dif(dim2,dim);
+                mat P4=Q.to_mat();
+                vec tmp;
                 for (i=0;i<dim2;i++){
-                        Xpred_dif.set_row(i, sqrt(w(i)) * (Xik.get_col(i) - xp));
+                        tmp = Xik.get_col(i)-xp;
+                        P4+=w(i)*outer_product(tmp,tmp);
                 }               
-                
-                // NEW Sigma points
-                mat tmp;
-                qr(concat_vertical(Xpred_dif,Q._Ch()), tmp);
-                mat Ppred=tmp.get_rows(0,dim-1);
-                // New Xi(k+1)
-                Xik.set_col(0,xp);
-                for ( i=0;i<dim; i++){
-                        vec tmp=sqrt(npk)*Ppred.get_col(i);
-                        Xik.set_col(i+1, _mu+tmp);
-                        Xik.set_col(i+1+dim, _mu-tmp);
-                }
-                for (i=0;i<dim2;i++){
-                        Xpred_dif.set_row(i, sqrt(w(i)) * (Xik.get_col(i) - xp));
-                }               
-                
-                
-                
+                                        
                 //step 5
                 mat Yi(dimy,dim2);
@@ -501 +559 @@
                 }
                 //step 7
-                mat Ypred_dif(dim2,dimy);
-                
-                for (i=0; i<dim2; i++){
-                        Ypred_dif.set_row(i, sqrt(w(i))*(Yi.get_col(i)-_yp));
-                }
-                if (!qr(concat_vertical(Ypred_dif,R._Ch()) ,tmp)){ bdm_warning("QR failed");}
-                _Ry._Ch() = tmp.get_rows(0,dimy-1);
+                mat Pvv=R.to_mat();
+                for (i=0;i<dim2;i++){
+                        tmp = Yi.get_col(i)-_yp;
+                        Pvv+=w(i)*outer_product(tmp,tmp);
+                }               
+                _Ry._Ch() = chol(Pvv);
                 
                 // step 8
-                mat Pxy=Xpred_dif.T()*Ypred_dif;
+                mat Pxy=zeros(dim,dimy);
+                for (i=0;i<dim2;i++){
+                        Pxy+=w(i)*outer_product(Xi.get_col(i)-xp, Yi.get_col(i)-_yp);
+                }               
                 mat iRy=inv(_Ry._Ch());
                 
@@ -516 +576 @@
                 mat K=Pxy*iRy*iRy.T();
                 mat K2=Pxy*inv(_Ry.to_mat());
-                
+                                
                 /////////////// new filtering density
                 _mu = xp + K*(yt - _yp);
+                
+                if ( _mu ( 3 ) >pi ) _mu ( 3 )-=2*pi;
+                if ( _mu ( 3 ) <-pi ) _mu ( 3 ) +=2*pi;
                 // fill the space in Ppred;
-                mat Rpred =concat_vertical(Xpred_dif - Ypred_dif*K.T(), _Ry._Ch()*K.T());
-                
-                if(!qr(Rpred,tmp)) {bdm_warning("QR failed");}
-                _P._Ch()=tmp.get_rows(0,dim-1);
+                _P._Ch()=chol(P4-K*_Ry.to_mat()*K.T());
         }
         void from_setting(const Setting &set){