...

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

diff --git a/lib/cpPlugins/Extensions/Algorithms/KalmanFilter.hxx b/lib/cpPlugins/Extensions/Algorithms/KalmanFilter.hxx
index 0e4f106e14cbffe760f97cf7ddbcedb80fe56331..a75fb7f6b833c4cdf4d0828de544571bbc55b6b5 100644 (file)
--- a/lib/cpPlugins/Extensions/Algorithms/KalmanFilter.hxx
+++ b/lib/cpPlugins/Extensions/Algorithms/KalmanFilter.hxx
@@ -7,20 +7,23 @@ 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 );
+  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 ar = 0; ar < Ar; ar++ )
+  for( unsigned int i = 1; i <= m; ++i )
   {
-    for( unsigned int ac = 0; ac < Ac; ac++ )
+    for( unsigned int j = 1; j <= n; ++j )
     {
-      typename M::element_type vA = A[ ar ][ ac ];
-      for( unsigned int br = 0; br < Br; br++ )
+      for( unsigned int k = 1; k <= p; ++k )
       {
-        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 ];
+        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