Changeset 271 for bdm/estim/libKF.cpp

Timestamp:

02/16/09 10:03:13 (15 years ago)

Author:

smidl

Message:

Next major revision

Files:

• bdm/estim/libKF.cpp (modified) (3 diffs)

bdm/estim/libKF.cpp

@@ -6 +6 @@
 using std::endl;
 
-KalmanFull::KalmanFull ( mat A0, mat B0, mat C0, mat D0, mat R0, mat Q0, mat P0, vec mu0 ) {
+KalmanFull::KalmanFull ( mat A0, mat B0, mat C0, mat D0, mat Q0, mat R0, mat P0, vec mu0 ) {
         dimx = A0.rows();
         dimu = B0.cols();
@@ -15 +15 @@
         it_assert_debug ( C0.cols() ==dimx, "KalmanFull: C is not square" );
         it_assert_debug ( ( D0.rows() ==dimy ) || ( D0.cols() ==dimu ), "KalmanFull: D is not compatible" );
+        it_assert_debug ( ( Q0.cols() ==dimx ) || ( Q0.rows() ==dimx ), "KalmanFull: Q is not compatible" );
         it_assert_debug ( ( R0.cols() ==dimy ) || ( R0.rows() ==dimy ), "KalmanFull: R is not compatible" );
-        it_assert_debug ( ( Q0.cols() ==dimx ) || ( Q0.rows() ==dimx ), "KalmanFull: Q is not compatible" );
 
         A = A0;
@@ -116 +116 @@
 
 
-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 );
+void KalmanCh::set_parameters ( const mat &A0,const mat &B0,const mat &C0,const mat &D0,const chmat &Q0,const chmat &R0 ) {
+
+        ( ( Kalman<chmat>* ) this )->set_parameters ( A0,B0,C0,D0,Q0,R0 );
         // Cholesky special!
-        preA.clear();
+        preA=zeros(dimy+dimx+dimx,dimy+dimx);
+//      preA.clear();
         preA.set_submatrix ( 0,0,R._Ch() );
         preA.set_submatrix ( dimy+dimx,dimy,Q._Ch() );