Index: library/bdm/estim/kalman.h
===================================================================
--- library/bdm/estim/kalman.h (revision 1154)
+++ library/bdm/estim/kalman.h (revision 1158)
@@ -362,5 +362,5 @@
 
     void validate() {
-        KalmanFull::validate();
+       // KalmanFull::validate();
 
         // check stats and IM and OM
@@ -426,4 +426,77 @@
 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,26 +541,11 @@
 	
 		//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,14 +559,16 @@
 		}
 		//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,12 +576,12 @@
 		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){
