yet another refactoring

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

diff --git a/lib/cpExtensions/Algorithms/KalmanFilter.hxx b/lib/cpExtensions/Algorithms/KalmanFilter.hxx
new file mode 100644 (file)
index 0000000..e81cae2
--- /dev/null
+++ b/lib/cpExtensions/Algorithms/KalmanFilter.hxx
@@ -0,0 +1,37 @@
+#ifndef __CPEXTENSIONS__ALGORITHMS__KALMANFILTER__HXX__
+#define __CPEXTENSIONS__ALGORITHMS__KALMANFILTER__HXX__
+
+// -------------------------------------------------------------------------
+template< typename T >
+template< class M >
+void cpExtensions::Algorithms::KalmanFilter< T >::
+Kronecker( M& AkB, const M& A, const M& B )
+{
+  unsigned int m = A.rows( ); unsigned int n = A.cols( );
+  unsigned int p = B.rows( ); unsigned int q = B.cols( );
+  AkB.set_size( m * p, n * q );
+
+  for( unsigned int i = 1; i <= m; ++i )
+  {
+    for( unsigned int j = 1; j <= n; ++j )
+    {
+      for( unsigned int k = 1; k <= p; ++k )
+      {
+        for( unsigned int l = 1; l <= q; ++l )
+        {
+          unsigned int al = ( ( p * ( i - 1 ) ) + k ) - 1;
+          unsigned int be = ( ( q * ( j - 1 ) ) + l ) - 1;
+          AkB[ al ][ be ] = A[ i - 1 ][ j - 1 ] * B[ k - 1 ][ l - 1 ];
+
+        } // rof
+
+      } // rof
+
+    } // rof
+
+  } // rof
+}
+
+#endif // __CPEXTENSIONS__ALGORITHMS__KALMANFILTER__HXX__
+
+// eof - $RCSfile$