Changeset 1154 for library

Timestamp:

07/28/10 10:45:59 (14 years ago)

Author:

smidl

Message:

UKF

Files:

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

library/bdm/estim/kalman.h

@@ -426 +426 @@
 SHAREDPTR ( EKFCh );
 
+class UKFCh : public EKFCh{
+        public:
+                double kappa;
+                
+        void bayes ( const vec &yt, const vec &cond = empty_vec ){
+                
+                vec &_mu = est._mu();
+                chmat &_P = est._R();
+                chmat &_Ry = fy._R();
+                vec &_yp = fy._mu();
+                
+                int dim = dimension();
+                int dim2 = 1+dim+dim;
+                
+                double npk =dim+kappa;//n+kappa
+                mat Xi(dim,dim2);
+                vec w=ones(dim2)* 0.5/npk;
+                w(0) = (npk-dim)/npk; // mean is special
+                
+                //step 1.
+                int i;
+                Xi.set_col(0,_mu); 
+                
+                for ( i=0;i<dim; i++){
+                        vec tmp=sqrt(npk)*_P._Ch().get_col(i);
+                        Xi.set_col(i+1, _mu+tmp);
+                        Xi.set_col(i+1+dim, _mu-tmp);
+                }
+                
+                // step 2.
+                mat Xik(dim,dim2);
+                for (i=0; i<dim2; i++){
+                        Xik.set_col(i, pfxu->eval(Xi.get_col(i), cond));
+                }
+                
+                //step 3
+                vec xp=zeros(dim);
+                for (i=0;i<dim2;i++){
+                        xp += w(i) * Xik.get_col(i);
+                }
+        
+                //step 4
+                mat Xpred_dif(dim2,dim);
+                for (i=0;i<dim2;i++){
+                        Xpred_dif.set_row(i, sqrt(w(i)) * (Xik.get_col(i) - xp));
+                }               
+                
+                // 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);
+                for (i=0; i<dim2; i++){
+                        Yi.set_col(i, phxu->eval(Xik.get_col(i), cond));
+                }
+                //step 6
+                _yp.clear();
+                for (i=0;i<dim2;i++){
+                        _yp += w(i) * Yi.get_col(i);
+                }
+                //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);
+                
+                // step 8
+                mat Pxy=Xpred_dif.T()*Ypred_dif;
+                mat iRy=inv(_Ry._Ch());
+                
+                //filtering????? -- correction
+                mat K=Pxy*iRy*iRy.T();
+                mat K2=Pxy*inv(_Ry.to_mat());
+                
+                /////////////// new filtering density
+                _mu = xp + K*(yt - _yp);
+                // 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);
+        }
+        void from_setting(const Setting &set){
+                EKFCh::from_setting(set);
+                kappa = 1.0;
+                UI::get(kappa,set,"kappa");
+        }
+};
+UIREGISTER(UKFCh);
 
 //////// INstance