Kalman filter added

[cpPlugins.git] / lib / cpPlugins / Extensions / Algorithms / KalmanFilter.hxx

diff --git a/lib/cpPlugins/Extensions/Algorithms/KalmanFilter.hxx b/lib/cpPlugins/Extensions/Algorithms/KalmanFilter.hxx
new file mode 100644 (file)
index 0000000..0e4f106
--- /dev/null
+++ b/lib/cpPlugins/Extensions/Algorithms/KalmanFilter.hxx
@@ -0,0 +1,34 @@
+#ifndef __CPPLUGINS__EXTENSIONS__ALGORITHMS__KALMANFILTER__HXX__
+#define __CPPLUGINS__EXTENSIONS__ALGORITHMS__KALMANFILTER__HXX__
+
+// -------------------------------------------------------------------------
+template< typename T >
+template< class M >
+void cpPlugins::Extensions::Algorithms::KalmanFilter< T >::
+Kronecker( M& AkB, const M& A, const M& B )
+{
+  unsigned int Ar = A.rows( ); unsigned int Ac = A.cols( );
+  unsigned int Br = B.rows( ); unsigned int Bc = B.cols( );
+  AkB.set_size( Ar * Br, Ac * Bc );
+
+  for( unsigned int ar = 0; ar < Ar; ar++ )
+  {
+    for( unsigned int ac = 0; ac < Ac; ac++ )
+    {
+      typename M::element_type vA = A[ ar ][ ac ];
+      for( unsigned int br = 0; br < Br; br++ )
+      {
+        unsigned int rOff = ar + ( br * ( Br - 1 ) );
+        for( unsigned int bc = 0; bc < Bc; bc++ )
+          AkB[ rOff ][ ac + ( bc * ( Bc - 1 ) ) ] = vA * B[ br ][ bc ];
+
+      } // rof
+
+    } // rof
+
+  } // rof
+}
+
+#endif // __CPPLUGINS__EXTENSIONS__ALGORITHMS__KALMANFILTER__HXX__
+
+// eof - $RCSfile$