Changeset 37 for bdm/estim

Timestamp:

03/14/08 18:11:21 (16 years ago)

Author:

smidl

Message:

Matrix in Cholesky decomposition, Square-root Kalman and many bug fixes

Location:

bdm/estim

Files:

• libKF.cpp (modified) (3 diffs)
• libKF.h (modified) (8 diffs)

bdm/estim/libKF.cpp

@@ -26 +26 @@
         mu = mu0;
         P = P0;
+        
+        ll = 0.0;
+        evalll=true;
 }
 
@@ -43 +46 @@
         P -= _K*C*P; // P = P -KCP;
         mu += _K* ( y-C*mu-D*u );
+
+        if ( evalll ) {
+                //from enorm
+                vec x=y-C*mu-D*u;
+                ll = -0.5* ( R.cols() * 1.83787706640935 +log ( det ( _Ry ) ) +x* ( _iRy*x ) );
+        }
 };
 
@@ -50 +59 @@
 }
 
+void KalmanCh::set_parameters ( const mat &A0,const mat &B0,const mat &C0,const mat &D0,const chmat &R0,const chmat &Q0 ) {
+
+        ( ( Kalman<chmat>* ) this )->set_parameters ( A0,B0,C0,D0,R0,Q0 );
+        // Cholesky special!
+        preA.clear();
+        preA.set_submatrix ( 0,0,R._Ch() );
+        preA.set_submatrix ( dimy+dimx,dimy,Q._Ch() );
+}
+
+
+void KalmanCh::bayes ( const vec &dt ) {
+
+        vec u = dt.get ( dimy,dimy+dimu-1 );
+        vec y = dt.get ( 0,dimy-1 );
+
+        //TODO get rid of Q in qr()!
+//      mat Q;
+
+        //R and Q are already set in set_parameters()
+        preA.set_submatrix ( dimy,0, ( _P->_Ch() ) *C.T() );
+        //Fixme can be more efficient if .T() is not used
+        preA.set_submatrix ( dimy,dimy, ( _P->_Ch() ) *A.T() );
+
+//      if ( !qr ( preA,Q,postA ) ) it_warning ( "QR in kalman unstable!" );
+        if ( !qr ( preA,postA ) ) it_warning ( "QR in kalman unstable!" );
+
+        ( _Ry->_Ch() ) =postA ( 0,dimy-1, 0,dimy-1 );
+        _K = ( postA ( 0,dimy-1 ,dimy,dimy+dimx-1 ) ).T();
+        ( _P->_Ch() ) = postA ( dimy,dimy+dimx-1,dimy,dimy+dimx-1 );
+        _Ry->inv ( *_iRy );
+        fy._cached ( true );
+        *_mu = A* ( *_mu ) + B*u + ( _K ) * ( _iRy->_Ch() ).T() * ( y-C* ( *_mu )-D*u );
+
+        /*      cout << "P:" <<_P->to_mat() <<endl;
+                cout << "Ry:" <<_Ry->to_mat() <<endl;
+                cout << "iRy:" <<_iRy->to_mat() <<endl;*/
+        if ( evalll==true ) { //likelihood of observation y
+                ll=fy.evalpdflog ( y );
+        }
+}
+
+
+EKFCh::EKFCh ( RV rvx0, RV rvy0, RV rvu0 ) : KalmanCh ( rvx0,rvy0,rvu0 ) {}
+
+void EKFCh::set_parameters ( diffbifn* pfxu0,  diffbifn* phxu0,const chmat Q0,const chmat R0 ) {
+        pfxu = pfxu0;
+        phxu = phxu0;
+
+        //initialize matrices A C, later, these will be only updated!
+        pfxu->dfdx_cond ( *_mu,zeros ( dimu ),A,true );
+//      pfxu->dfdu_cond ( *_mu,zeros ( dimu ),B,true );
+        B.clear();
+        phxu->dfdx_cond ( *_mu,zeros ( dimu ),C,true );
+//      phxu->dfdu_cond ( *_mu,zeros ( dimu ),D,true );
+        D.clear();
+
+        R = R0;
+        Q = Q0;
+
+        // Cholesky special!
+        preA.clear();
+        preA.set_submatrix ( 0,0,R._Ch() );
+        preA.set_submatrix ( dimy+dimx,dimy,Q._Ch() );
+}
+
+
+void EKFCh::bayes ( const vec &dt ) {
+
+        vec u = dt.get ( dimy,dimy+dimu-1 );
+        vec y = dt.get ( 0,dimy-1 );
+
+        //TODO get rid of Q in qr()!
+//      mat Q;
+
+        //R and Q are already set in set_parameters()
+        preA.set_submatrix ( dimy,0, ( _P->_Ch() ) *C.T() );
+        //Fixme can be more efficient if .T() is not used
+        preA.set_submatrix ( dimy,dimy, ( _P->_Ch() ) *A.T() );
+
+//      if ( !qr ( preA,Q,postA ) ) it_warning ( "QR in kalman unstable!" );
+        if ( !qr ( preA,postA ) ) it_warning ( "QR in kalman unstable!" );
+
+        ( _Ry->_Ch() ) =postA ( 0,dimy-1, 0,dimy-1 );
+        _K = ( postA ( 0,dimy-1 ,dimy,dimy+dimx-1 ) ).T();
+        ( _P->_Ch() ) = postA ( dimy,dimy+dimx-1,dimy,dimy+dimx-1 );
+        _Ry->inv ( *_iRy );
+        fy._cached ( true );
+        *_yp = phxu->eval ( *_mu,u );
+        *_mu = pfxu->eval ( *_mu ,u ) + ( _K ) * ( _iRy->_Ch() ).T() * ( y-*_yp );
+
+        /*      cout << "P:" <<_P->to_mat() <<endl;
+                cout << "Ry:" <<_Ry->to_mat() <<endl;
+                cout << "iRy:" <<_iRy->to_mat() <<endl;*/
+
+        if ( evalll==true ) { //likelihood of observation y
+                ll=fy.evalpdflog ( y );
+        }
+}
+
 void KFcondQR::condition ( const vec &QR ) {
         it_assert_debug ( QR.length() == ( rvc.count() ),"KFcondRQ: conditioning by incompatible vector" );
 
-        Q.setD ( QR ( 0, dimx-1 ));
-        R.setD ( QR ( dimx, -1 ));
+        Q.setD ( QR ( 0, dimx-1 ) );
+        R.setD ( QR ( dimx, -1 ) );
 };
 

bdm/estim/libKF.h

@@ -17 +17 @@
 #include "../stat/libFN.h"
 #include "../stat/libEF.h"
-
+#include "../math/chmat.h"
 
 using namespace itpp;
@@ -38 +38 @@
         mat P;
 
+        bool evalll;
+        double ll;
 public:
         //! Full constructor
@@ -124 +126 @@
 };
 
+/*! \brief Kalman filter in square root form
+*/
+class KalmanCh : public Kalman<chmat>{
+protected:
+//! pre array (triangular matrix)
+mat preA;
+//! post array (triangular matrix)
+mat postA;
+
+public:
+        //! Default constructor
+        KalmanCh ( RV rvx0, RV rvy0, RV rvu0 ):Kalman<chmat>(rvx0,rvy0,rvu0),preA(dimy+dimx+dimx,dimy+dimx),postA(dimy+dimx,dimy+dimx){};
+        //! Set parameters with check of relevance
+        void set_parameters ( const mat &A0,const mat &B0,const mat &C0,const mat &D0,const chmat &R0,const chmat &Q0 );
+        void set_est ( const vec &mu0, const chmat &P0 ) {
+                est.set_parameters ( mu0,P0 );
+        };
+        
+        
+        /*!\brief  Here dt = [yt;ut] of appropriate dimensions
+        
+        The following equality hold::\f[
+\left[\begin{array}{cc}
+R^{0.5}\\
+P_{t|t-1}^{0.5}C' & P_{t|t-1}^{0.5}CA'\\
+ & Q^{0.5}\end{array}\right]<\mathrm{orth.oper.}>=\left[\begin{array}{cc}
+R_{y}^{0.5} & KA'\\
+ & P_{t+1|t}^{0.5}\\
+\\\end{array}\right]\f]
+
+Thus this objevt evaluates only predictors! Not filtering densities.
+        */
+        void bayes ( const vec &dt );
+};
+
 /*!
 \brief Extended Kalman Filter
@@ -130 +167 @@
 */
 template<class sq_T>
-
 class EKF : public Kalman<ldmat> {
         //! Internal Model f(x,u)
@@ -146 +182 @@
 
 /*!
+\brief Extended Kalman Filter in Square roor
+
+An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
+*/
+
+class EKFCh : public KalmanCh {
+        //! Internal Model f(x,u)
+        diffbifn* pfxu;
+        //! Observation Model h(x,u)
+        diffbifn* phxu;
+public:
+        //! Default constructor
+        EKFCh ( RV rvx, RV rvy, RV rvu );
+        //! Set nonlinear functions for mean values and covariance matrices.
+        void set_parameters ( diffbifn* pfxu, diffbifn* phxu, const chmat Q0, const chmat R0 );
+        //! Here dt = [yt;ut] of appropriate dimensions
+        void bayes ( const vec &dt );
+};
+
+/*!
 \brief Kalman Filter with conditional diagonal matrices R and Q.
 */
@@ -176 +232 @@
 Kalman<sq_T>::Kalman ( const Kalman<sq_T> &K0 ) : BM ( K0.rv ),rvy ( K0.rvy ),rvu ( K0.rvu ),
                 dimx ( rv.count() ), dimy ( rvy.count() ),dimu ( rvu.count() ),
-                A ( dimx,dimx ), B ( dimx,dimu ), C ( dimy,dimx ), D ( dimy,dimu ),est ( rv ), fy ( rvy ) {
+                A ( dimx,dimx ), B ( dimx,dimu ), C ( dimy,dimx ), D ( dimy,dimu ),
+                Q(dimx), R(dimy),
+                est ( rv ), fy ( rvy ) {
 
         this->set_parameters ( K0.A, K0.B, K0.C, K0.D, K0.R, K0.Q );
@@ -188 +246 @@
         fy._R ( _Ry,_iRy );
 
-//reset copy values in pointers
+// copy values in pointers
         *_mu = *K0._mu;
         *_P = *K0._P;
@@ -272 +330 @@
 
 template<class sq_T>
-EKF<sq_T>::EKF ( RV rvx0, RV rvy0, RV rvu0 ) : Kalman<ldmat> ( rvx0,rvy0,rvu0 ) {}
+EKF<sq_T>::EKF ( RV rvx0, RV rvy0, RV rvu0 ) : Kalman<sq_T> ( rvx0,rvy0,rvu0 ) {}
 
 template<class sq_T>