Changeset 744 for library/bdm/estim/ekf_template.h

Timestamp:

11/28/09 14:38:48 (15 years ago)

Author:

smidl

Message:

Working unitsteps and controlloop + corresponding fixes

Files:

• library/bdm/estim/ekf_template.h (modified) (3 diffs)

library/bdm/estim/ekf_template.h

@@ -22 +22 @@
 public:
         void condition ( const vec &QR0 ) {
-                Q = diag ( QR0 ( 0, dimx - 1 ) );
-                R = diag ( QR0 ( dimx, dimx + dimy - 1 ) );
+                Q = diag ( QR0 ( 0, dimension() - 1 ) );
+                R = diag ( QR0 ( dimension(), dimension() + dimy - 1 ) );
         };
 };
@@ -30 +30 @@
 class EKFCh_dQ : public EKFCh {
 public:
-        void condition ( const vec &Q0 ) {
-                Q.setD ( Q0, 0 );
+        BM* _copy_() const {return new EKFCh_dQ(*this);}
+        //! new bayes, expects cond = [ut, Qt]
+        void bayes( const vec &yt , const vec &cond) {
+                vec ut=cond.left(dimc-dimension());
+                vec dQt=cond.right(dimension());
+                Q.setD ( dQt, 0 );
                 //from EKF
-                preA.set_submatrix ( dimy + dimx, dimy, Q._Ch() );
+                preA.set_submatrix ( dimy + dimension(), dimy, Q._Ch() );
+                
+                EKFCh::bayes(yt,ut);
         };
+        void validate() {
+                EKFCh::validate();
+                dimc += dimension();
+        }
 };
 
@@ -43 +53 @@
                 Q.setCh ( chQ0 );
                 //from EKF
-                preA.set_submatrix ( dimy + dimx, dimy, Q._Ch() );
+                preA.set_submatrix ( dimy + dimension(), dimension(), Q._Ch() );
         };
 };